blob: 717778a6bd14d54fc5b4cae434a31bcb0eb47ecd (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
|
#include "io.h"
#include "../shared/consts.h"
#include "../shared/errcatch.h"
#include "hypervisor.h"
#include "modes.h"
#include "orangutan_shim.h"
#include <stdio.h>
bool g_w2_io_object_detected = false;
void w2_io_object_detection() {
unsigned int front_distance = analog_read(W2_FRONT_SENSOR_PIN);
if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_HALT) return;
if (!g_w2_io_object_detected && front_distance >= W2_IO_DISTANCE_CLOSE_THRESHOLD) {
g_w2_io_object_detected = true;
w2_hypervisor_time_start(W2_TIMER_OBJECT_DETECTION);
w2_errcatch_throw(W2_E_WARN_OBSTACLE_DETECTED);
}
if (g_w2_io_object_detected) {
if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) g_w2_io_object_detected = false;
if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT)
w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK);
set_motors(0, 0);
}
}
void w2_io_main() {
w2_io_object_detection();
// TODO: battery status
return;
}
|