diff options
| -rw-r--r-- | robot/errcatch.c | 16 | ||||
| -rw-r--r-- | robot/hypervisor.c | 49 | ||||
| -rw-r--r-- | robot/hypervisor.h | 16 | ||||
| -rw-r--r-- | robot/io.c | 21 | ||||
| -rw-r--r-- | robot/sercomm.c | 10 |
5 files changed, 69 insertions, 43 deletions
diff --git a/robot/errcatch.c b/robot/errcatch.c index 3830d54..ed4de51 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -5,17 +5,29 @@ #include "orangutan_shim.h" #include "sercomm.h" +// #include <stdio.h> +// unsigned int g_w2_err_index = 0; +// unsigned int g_w2_err_disp[4] = {0}; + void w2_errcatch_handle_error(w2_s_error *error) { uint8_t severity = error->code & W2_E_TYPE_MASK; +// clear(); +// g_w2_err_disp[g_w2_err_index++] = error->code; +// char disp[32]; +// sprintf(disp, "%02x %02x", g_w2_err_disp[0], g_w2_err_disp[1]); +// print(disp); +// lcd_goto_xy(0, 1); +// sprintf(disp, "%02x %02x", g_w2_err_disp[2], g_w2_err_disp[3]); +// print(disp); + // trigger emergency mode for critical errors if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_call(W2_M_HALT); // TODO: handle more error types switch (error->code) { - case W2_E_WARN_UNCAUGHT_ERROR: { + case W2_E_WARN_UNCAUGHT_ERROR: break; - } case W2_E_WARN_OBSTACLE_DETECTED: break; case W2_E_CRIT_OBSTACLE_STUCK: diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 50dc1ac..580ff55 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -7,15 +7,14 @@ #include "sercomm.h" uint64_t g_w2_hypervisor_cycles = 0; -uint64_t g_w2_hypervisor_uptime_ms = 0; -unsigned long g_w2_hypervisor_ema_sercomm_ms = 0; -unsigned long g_w2_hypervisor_ema_errcatch_ms = 0; -unsigned long g_w2_hypervisor_ema_io_ms = 0; -unsigned long g_w2_hypervisor_ema_mode_ms = 0; +uint64_t g_w2_hypervisor_uptime_qs = 0; +unsigned long g_w2_hypervisor_ema_sercomm_qs = 0; +unsigned long g_w2_hypervisor_ema_errcatch_qs = 0; +unsigned long g_w2_hypervisor_ema_io_qs = 0; +unsigned long g_w2_hypervisor_ema_mode_qs = 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_cpu_ticks = 0; +uint64_t g_w2_hypervisor_qs_timer_offset = 0; unsigned int g_w2_ping_ms = 0; uint8_t g_w2_ping_id = 0; @@ -31,27 +30,27 @@ void w2_hypervisor_main() { if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n"); #endif - g_w2_hypervisor_uptime_ms += w2_get_ms(); + g_w2_hypervisor_uptime_qs += w2_get_qs(); w2_time_reset(); w2_sercomm_main(); - unsigned long sercomm_time = w2_get_ms(); + unsigned long sercomm_time = w2_get_qs(); w2_errcatch_main(); - unsigned long errcatch_time = w2_get_ms() - sercomm_time; + unsigned long errcatch_time = w2_get_qs() - sercomm_time; w2_io_main(); - unsigned long io_time = w2_get_ms() - errcatch_time; + unsigned long io_time = w2_get_qs() - errcatch_time; w2_modes_main(); - unsigned long mode_time = w2_get_ms() - io_time; + unsigned long mode_time = w2_get_qs() - io_time; // calculate exponential moving averages - g_w2_hypervisor_ema_sercomm_ms = - w2_util_exp_mov_avg(g_w2_hypervisor_ema_sercomm_ms, sercomm_time); - g_w2_hypervisor_ema_errcatch_ms = - w2_util_exp_mov_avg(g_w2_hypervisor_ema_errcatch_ms, errcatch_time); - g_w2_hypervisor_ema_io_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_io_ms, io_time); - g_w2_hypervisor_ema_mode_ms = w2_util_exp_mov_avg(g_w2_hypervisor_ema_mode_ms, mode_time); + g_w2_hypervisor_ema_sercomm_qs = + w2_util_exp_mov_avg(g_w2_hypervisor_ema_sercomm_qs, sercomm_time); + g_w2_hypervisor_ema_errcatch_qs = + w2_util_exp_mov_avg(g_w2_hypervisor_ema_errcatch_qs, errcatch_time); + g_w2_hypervisor_ema_io_qs = w2_util_exp_mov_avg(g_w2_hypervisor_ema_io_qs, io_time); + g_w2_hypervisor_ema_mode_qs = w2_util_exp_mov_avg(g_w2_hypervisor_ema_mode_qs, mode_time); - if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); + if ((mode_time / 1e3) > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED); #ifdef W2_SIM if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); @@ -63,16 +62,20 @@ void w2_hypervisor_main() { g_w2_hypervisor_cycles++; } +uint64_t w2_get_qs() { + return ticks_to_microseconds(get_ticks()) - g_w2_hypervisor_qs_timer_offset; +} + uint64_t w2_get_ms() { - return ticks_to_microseconds(get_ticks() - g_w2_hypervisor_ms_timer_offset) / 1e3; + return w2_get_qs() / 1e3; } -void w2_time_reset() { g_w2_hypervisor_ms_timer_offset = get_ticks(); } +void w2_time_reset() { g_w2_hypervisor_qs_timer_offset = ticks_to_microseconds(get_ticks()); } void w2_hypervisor_time_start(uint8_t label) { - g_w2_hypervisor_timers[label] = g_w2_hypervisor_uptime_ms; + g_w2_hypervisor_timers[label] = g_w2_hypervisor_uptime_qs; } uint64_t w2_hypervisor_time_end(uint8_t label) { - return g_w2_hypervisor_uptime_ms - g_w2_hypervisor_timers[label]; + return (g_w2_hypervisor_uptime_qs - g_w2_hypervisor_timers[label]) / 1e3; } diff --git a/robot/hypervisor.h b/robot/hypervisor.h index 154ae58..ba58977 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -14,12 +14,12 @@ #define W2_TIMER_OBJECT_DETECTION 1 extern uint64_t g_w2_hypervisor_cycles; -extern uint64_t g_w2_hypervisor_uptime_ms; +extern uint64_t g_w2_hypervisor_uptime_qs; -extern unsigned long g_w2_hypervisor_ema_sercomm_ms; -extern unsigned long g_w2_hypervisor_ema_errcatch_ms; -extern unsigned long g_w2_hypervisor_ema_io_ms; -extern unsigned long g_w2_hypervisor_ema_mode_ms; +extern unsigned long g_w2_hypervisor_ema_sercomm_qs; +extern unsigned long g_w2_hypervisor_ema_errcatch_qs; +extern unsigned long g_w2_hypervisor_ema_io_qs; +extern unsigned long g_w2_hypervisor_ema_mode_qs; extern unsigned int g_w2_ping_ms; extern uint8_t g_w2_ping_id; @@ -39,10 +39,12 @@ void w2_hypervisor_main(); /** * pololu's `get_ms()` estimates run time by only adding the CPU time taken up * by pololu library functions. this function uses the `get_ticks()` function - * to calculate elapsed milliseconds, and should therefore be more accurate. + * to calculate elapsed microseconds, and should therefore be more accurate. */ +uint64_t w2_get_qs(); +/** return uptime in milliseconds (less accurate) */ uint64_t w2_get_ms(); -/** reset time returned by `w2_get_ms()` */ +/** reset time returned by `w2_get_qs()` */ void w2_time_reset(); /** start timer with label `label` */ @@ -5,25 +5,34 @@ #include "modes.h" #include "orangutan_shim.h" -#include <stdio.h> - bool g_w2_io_object_detected = false; +#include <stdio.h> + void w2_io_object_detection() { - unsigned int front_distance = analog_read(W2_FRONT_SENSOR_PIN); + 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); - w2_errcatch_throw(W2_E_WARN_OBSTACLE_DETECTED); + 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; - if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT) + 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); } } diff --git a/robot/sercomm.c b/robot/sercomm.c index 195c477..a04d35f 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -154,11 +154,11 @@ void w2_cmd_info_rx(w2_s_bin *data) { W2_CREATE_MSG_BIN(w2_s_cmd_info_tx, res_msg, res_bin); res_msg->opcode = W2_CMD_INFO | W2_CMDDIR_TX; strncpy((char *)res_msg->build_str, W2_BUILD_STR, sizeof(res_msg->build_str)); - res_msg->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms; - res_msg->io_ms = (uint8_t)g_w2_hypervisor_ema_io_ms; - res_msg->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_ms; - res_msg->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; - res_msg->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); + res_msg->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_qs / 1e3; + res_msg->io_ms = (uint8_t)g_w2_hypervisor_ema_io_qs / 1e3; + res_msg->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_qs / 1e3; + res_msg->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_qs / 1e3; + res_msg->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_qs / 1e6)); res_msg->mode = g_w2_mode_history[g_w2_mode_history_index]; res_msg->battery_mv = w2_bin_hton16(read_battery_millivolts()); |