blob: 42c3a4bfdddba3295b996c4b2209b09b3bfa60e5 (
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
37
38
39
40
41
42
43
44
45
|
#include "io.h"
#include "../shared/consts.h"
#include "../shared/errcatch.h"
#include "hypervisor.h"
#include "modes.h"
#include "orangutan_shim.h"
bool g_w2_io_object_detected = false;
#include <stdio.h>
void w2_io_object_detection() {
unsigned int front_distance = analog_read(W2_SIDE_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);
lcd_goto_xy(0, 0);
print("detected");
}
if (g_w2_io_object_detected) {
if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) {
g_w2_io_object_detected = false;
lcd_goto_xy(0, 0);
print(" ");
}
if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT) {
w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK);
lcd_goto_xy(0, 0);
print("unavoid ");
}
set_motors(0, 0);
}
}
void w2_io_main() {
w2_io_object_detection();
// TODO: battery status
return;
}
|