aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--robot/errcatch.c9
-rw-r--r--robot/hypervisor.c6
-rw-r--r--robot/hypervisor.h1
-rw-r--r--robot/io.c18
-rw-r--r--robot/io.h1
-rw-r--r--robot/mode_dirc.c4
6 files changed, 18 insertions, 21 deletions
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 9aeae9e..3830d54 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -16,9 +16,12 @@ void w2_errcatch_handle_error(w2_s_error *error) {
case W2_E_WARN_UNCAUGHT_ERROR: {
break;
}
- case W2_E_WARN_OBSTACLE_DETECTED: break;
- case W2_E_CRIT_OBSTACLE_STUCK: break;
- case W2_E_WARN_MODE_HISTORY_BUFFER_IOB: break;
+ case W2_E_WARN_OBSTACLE_DETECTED:
+ break;
+ case W2_E_CRIT_OBSTACLE_STUCK:
+ break;
+ case W2_E_WARN_MODE_HISTORY_BUFFER_IOB:
+ break;
default: {
g_w2_error_uncaught = true;
#ifdef W2_SIM
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 8b9d727..be3fb77 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -14,7 +14,7 @@ unsigned long g_w2_hypervisor_ema_io_ms = 0;
unsigned long g_w2_hypervisor_ema_mode_ms = 0;
uint64_t g_w2_hypervisor_timers[W2_HYPERVISOR_TIMER_COUNT] = {0};
-uint64_t g_w2_hypervisor_ms_timer_offset = 0;
+uint64_t g_w2_hypervisor_ms_timer_offset = 0;
uint64_t g_w2_hypervisor_ms_timer_cpu_ticks = 0;
unsigned int g_w2_ping_ms = 0;
@@ -65,9 +65,7 @@ uint64_t w2_get_ms() {
return ticks_to_microseconds(get_ticks() - g_w2_hypervisor_ms_timer_offset) / 1e3;
}
-void w2_time_reset() {
- g_w2_hypervisor_ms_timer_offset = get_ticks();
-}
+void w2_time_reset() { g_w2_hypervisor_ms_timer_offset = get_ticks(); }
void w2_hypervisor_time_start(uint8_t label) {
g_w2_hypervisor_timers[label] = g_w2_hypervisor_uptime_ms;
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index fe9dbea..e9699cf 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -46,4 +46,3 @@ void w2_time_reset();
void w2_hypervisor_time_start(uint8_t label);
/** stop timer with label `label` */
uint64_t w2_hypervisor_time_end(uint8_t label);
-
diff --git a/robot/io.c b/robot/io.c
index 9e0eae1..717778a 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -1,15 +1,15 @@
#include "io.h"
#include "../shared/consts.h"
#include "../shared/errcatch.h"
+#include "hypervisor.h"
#include "modes.h"
#include "orangutan_shim.h"
-#include "hypervisor.h"
#include <stdio.h>
bool g_w2_io_object_detected = false;
-void w2_io_main() {
+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;
@@ -21,16 +21,16 @@ void w2_io_main() {
}
if (g_w2_io_object_detected) {
- if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) {
- w2_errcatch_throw(0x55);
- g_w2_io_object_detected = false;
- }
- if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT) {
- w2_errcatch_throw(0x56);
+ 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;
}
diff --git a/robot/io.h b/robot/io.h
index 14ffb38..31fe410 100644
--- a/robot/io.h
+++ b/robot/io.h
@@ -8,4 +8,3 @@ extern bool g_w2_io_object_detected;
/** @brief i/o module main */
void w2_io_main();
-
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 429d1e1..d9c0d17 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -11,6 +11,4 @@
int16_t g_w2_mode_dirc_motor_l = 0;
int16_t g_w2_mode_dirc_motor_r = 0;
-void w2_mode_dirc() {
- set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r);
-}
+void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); }