diff options
| -rw-r--r-- | robot/errcatch.c | 9 | ||||
| -rw-r--r-- | robot/hypervisor.c | 6 | ||||
| -rw-r--r-- | robot/hypervisor.h | 1 | ||||
| -rw-r--r-- | robot/io.c | 18 | ||||
| -rw-r--r-- | robot/io.h | 1 | ||||
| -rw-r--r-- | robot/mode_dirc.c | 4 |
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); - @@ -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; } @@ -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); } |