diff options
| -rw-r--r-- | client/main.h | 1 | ||||
| -rw-r--r-- | client/serial.c | 2 | ||||
| -rw-r--r-- | client/ui_dirc.c | 4 | ||||
| -rw-r--r-- | robot/errcatch.c | 3 | ||||
| -rw-r--r-- | robot/hypervisor.c | 25 | ||||
| -rw-r--r-- | robot/hypervisor.h | 15 | ||||
| -rw-r--r-- | robot/io.c | 51 | ||||
| -rw-r--r-- | robot/io.h | 6 | ||||
| -rw-r--r-- | robot/mode_dirc.c | 11 | ||||
| -rw-r--r-- | robot/sercomm.c | 16 | ||||
| -rw-r--r-- | shared/consts.h | 27 | ||||
| -rw-r--r-- | shared/io.h | 59 | ||||
| -rw-r--r-- | shared/protocol.c | 5 | ||||
| -rw-r--r-- | shared/protocol.h | 14 |
14 files changed, 81 insertions, 158 deletions
diff --git a/client/main.h b/client/main.h index cd81348..b72b507 100644 --- a/client/main.h +++ b/client/main.h @@ -17,7 +17,6 @@ typedef struct { uint8_t map_height; w2_s_cmd_info_tx info; - w2_s_cmd_sens_tx io; } w2_s_client_state; extern w2_s_client_state g_w2_state; diff --git a/client/serial.c b/client/serial.c index 044b636..2a4d26f 100644 --- a/client/serial.c +++ b/client/serial.c @@ -38,7 +38,6 @@ void w2_cmd_mode_tx(w2_s_bin *data) { } void w2_cmd_cord_tx(w2_s_bin *data) {} void w2_cmd_bomd_tx(w2_s_bin *data) {} -void w2_cmd_sens_tx(w2_s_bin *data) {} void w2_cmd_info_tx(w2_s_bin *data) { memcpy(&g_w2_state.info, data->data, sizeof(w2_s_cmd_info_tx)); @@ -51,7 +50,6 @@ void w2_cmd_cord_rx(w2_s_bin *data) { return; } void w2_cmd_bomd_rx(w2_s_bin *data) { return; } void w2_cmd_sres_rx(w2_s_bin *data) { return; } void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_cmd_sens_rx(w2_s_bin *data) { return; } void w2_cmd_info_rx(w2_s_bin *data) { return; } void w2_cmd_disp_rx(w2_s_bin *data) { return; } void w2_cmd_play_rx(w2_s_bin *data) { return; } diff --git a/client/ui_dirc.c b/client/ui_dirc.c index e2da863..db91661 100644 --- a/client/ui_dirc.c +++ b/client/ui_dirc.c @@ -98,10 +98,6 @@ void w2_ui_onkey_dirc(int ch) { if (ch == 'q' || ch == 'w') g_w2_rf++; if (ch == 'a' || ch == 's') g_w2_rb++; if (ch == ' ') w2_send_mode(W2_M_DIRC); - - char buf[32]; - sprintf(buf, "er is iets fout, %02x", ch); - w2_errcatch_throw_msg(0x69, 32, buf); } void w2_ui_tab_dirc(bool first) { diff --git a/robot/errcatch.c b/robot/errcatch.c index 17c96fa..9aeae9e 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -16,6 +16,9 @@ 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; default: { g_w2_error_uncaught = true; #ifdef W2_SIM diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 2a6120b..8b9d727 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -14,6 +14,9 @@ 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_cpu_ticks = 0; + unsigned int g_w2_ping_ms = 0; uint8_t g_w2_ping_id = 0; bool g_w2_ping_received = true; @@ -26,17 +29,17 @@ void w2_hypervisor_main() { if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n"); #endif - g_w2_hypervisor_uptime_ms += get_ms(); - time_reset(); + g_w2_hypervisor_uptime_ms += w2_get_ms(); + w2_time_reset(); w2_sercomm_main(); - unsigned long sercomm_time = get_ms(); + unsigned long sercomm_time = w2_get_ms(); w2_errcatch_main(); - unsigned long errcatch_time = get_ms() - sercomm_time; - // w2_io_main(); - unsigned long io_time = get_ms() - errcatch_time; + unsigned long errcatch_time = w2_get_ms() - sercomm_time; + w2_io_main(); + unsigned long io_time = w2_get_ms() - errcatch_time; w2_modes_main(); - unsigned long mode_time = get_ms() - io_time; + unsigned long mode_time = w2_get_ms() - io_time; // calculate exponential moving averages g_w2_hypervisor_ema_sercomm_ms = @@ -58,6 +61,14 @@ void w2_hypervisor_main() { g_w2_hypervisor_cycles++; } +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_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 4b1ed9b..fe9dbea 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -7,9 +7,10 @@ #include "../shared/bool.h" /** amount of parallel timers */ -#define W2_HYPERVISOR_TIMER_COUNT (1) +#define W2_HYPERVISOR_TIMER_COUNT 2 -#define W2_TIMER_PING (0) +#define W2_TIMER_PING 0 +#define W2_TIMER_OBJECT_DETECTION 1 extern uint64_t g_w2_hypervisor_cycles; extern uint64_t g_w2_hypervisor_uptime_ms; @@ -32,7 +33,17 @@ extern bool g_w2_connected; */ 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. + */ +uint64_t w2_get_ms(); +/** reset time returned by `w2_get_ms()` */ +void w2_time_reset(); + /** start timer with label `label` */ void w2_hypervisor_time_start(uint8_t label); /** stop timer with label `label` */ uint64_t w2_hypervisor_time_end(uint8_t label); + @@ -1,35 +1,36 @@ -#include <string.h> - -#include "../shared/consts.h" #include "io.h" +#include "../shared/consts.h" +#include "../shared/errcatch.h" #include "modes.h" #include "orangutan_shim.h" +#include "hypervisor.h" + +#include <stdio.h> -w2_s_io_all g_w2_io; +bool g_w2_io_object_detected = false; void w2_io_main() { - g_w2_io.button[0].pressed = get_single_debounced_button_press(BUTTON_A); - g_w2_io.button[1].pressed = get_single_debounced_button_press(BUTTON_B); - g_w2_io.button[2].pressed = get_single_debounced_button_press(BUTTON_C); - g_w2_io.button[3].pressed = get_single_debounced_button_press(TOP_BUTTON); - g_w2_io.button[4].pressed = get_single_debounced_button_press(BOTTOM_BUTTON); + unsigned int front_distance = analog_read(W2_FRONT_SENSOR_PIN); - unsigned int sensor_values[5]; - qtr_read(sensor_values, QTR_EMITTERS_ON); - for (int i = 0; i < 5; i++) g_w2_io.qtr[i].range = sensor_values[i]; + if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_HALT) return; - // TODO average voltage over mutiple samples sensor - g_w2_io.battery.charge_level = analog_read(W2_BATTERY_PIN); - g_w2_io.front_distance.detection = analog_read(W2_FRONT_SENSOR_PIN); - g_w2_io.side_distance.detection = analog_read(W2_SIDE_SENSOR_PIN); + 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); + } - set_motors(g_w2_io.motor_left.speed, g_w2_io.motor_right.speed); - red_led(g_w2_io.led_red.on); - green_led(g_w2_io.led_green.on); + 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); + w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK); + } + set_motors(0, 0); + } - // TODO don't do this every cycle - char text_copy[17]; - memcpy((char *)text_copy, g_w2_io.lcd.text, 16); - text_copy[16] = 0x00; - print(text_copy); -}; + return; +} @@ -1,11 +1,11 @@ #pragma once +#include "../shared/bool.h" + /** @file io.h */ -#include "../shared/io.h" +extern bool g_w2_io_object_detected; /** @brief i/o module main */ void w2_io_main(); -/** @brief global struct containing all i/o */ -extern w2_s_io_all g_w2_io; diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 7021721..429d1e1 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -10,16 +10,7 @@ int16_t g_w2_mode_dirc_motor_l = 0; int16_t g_w2_mode_dirc_motor_r = 0; -uint8_t g_w2_mode_dirc_power = 100; void w2_mode_dirc() { - if (g_w2_connected == 1) - g_w2_mode_dirc_power = 100; - else - g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 1); - - if (g_w2_mode_dirc_power == 0) w2_modes_call(W2_M_HALT); - - set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power / 100, - g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power / 100); + set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); } diff --git a/robot/sercomm.c b/robot/sercomm.c index f52eb7e..efad449 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -139,22 +139,6 @@ void w2_cmd_sres_rx(w2_s_bin *data) { void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_cmd_sens_rx(w2_s_bin *data) { - W2_CREATE_MSG_BIN(w2_s_cmd_sens_tx, res_msg, res_bin); - res_msg->opcode = W2_CMD_SENS | W2_CMDDIR_TX; - memcpy((uint8_t *)&res_msg->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); - - for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&res_msg->io.qtr[i].range); - w2_bin_repl_hton16(&res_msg->io.front_distance.detection); - w2_bin_repl_hton16(&res_msg->io.side_distance.detection); - w2_bin_repl_hton16(&res_msg->io.battery.charge_level); - w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_left.speed); - w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_right.speed); - - w2_sercomm_append_msg(res_bin); - free(res_bin); -} - 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; diff --git a/shared/consts.h b/shared/consts.h index 11e6d14..9c2358f 100644 --- a/shared/consts.h +++ b/shared/consts.h @@ -4,7 +4,7 @@ #ifndef W2_BUILD_STR // is defined by CFLAGS += -DW2_BUILD_STR in makefile -#define W2_BUILD_STR ("????????") +#define W2_BUILD_STR "????????" #endif #if !defined W2_HOST_WIN32 && !defined W2_HOST_LINUX @@ -13,32 +13,39 @@ #endif /** serial baud rate (bit/s) */ -#define W2_SERIAL_BAUD (9600) +#define W2_SERIAL_BAUD 9600 /** size of input (receive) buffer (in bytes) */ -#define W2_SERIAL_READ_BUFFER_SIZE (255) +#define W2_SERIAL_READ_BUFFER_SIZE 255 /** size of the error handling buffer (in errors, not bytes) */ -#define W2_ERROR_BUFFER_SIZE (16) +#define W2_ERROR_BUFFER_SIZE 16 /** size of the serial communication buffer (in messages, not bytes) */ -#define W2_SERCOMM_BUFFER_SIZE (16) +#define W2_SERCOMM_BUFFER_SIZE 16 /** size of mode history buffer */ -#define W2_MODE_HISTORY_BUFFER_SIZE (8) +#define W2_MODE_HISTORY_BUFFER_SIZE 8 /** max logic module execution time in milliseconds */ -#define W2_MAX_MODULE_CYCLE_MS (20) +#define W2_MAX_MODULE_CYCLE_MS 20 /** exponential moving average new measurement weight (double 0-1) */ -#define W2_EMA_WEIGHT (0.10) +#define W2_EMA_WEIGHT 0.10 /** minimal time between pings */ -#define W2_PING_FREQUENCY (1e3) +#define W2_PING_FREQUENCY 1e3 /** max time between ping and answer */ -#define W2_PING_TIMEOUT (5e3) +#define W2_PING_TIMEOUT 5e3 /** default map width/height */ #define W2_MAP_DEFAULT_HEIGHT 5 #define W2_MAP_DEFAULT_WIDTH 5 +/** distance too close schmitt trigger low threshold */ +#define W2_IO_DISTANCE_CLOSE_THRESHOLD 400 +/** distance too close schmitt trigger high threshold */ +#define W2_IO_DISTANCE_FAR_THRESHOLD 100 +/** go into emergency mode if object still present after n ms */ +#define W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT 5e3 + /** front-facing distance sensor pinout */ #define W2_FRONT_SENSOR_PIN 5 /** battery voltage sensor pinout */ diff --git a/shared/io.h b/shared/io.h deleted file mode 100644 index 584ad1e..0000000 --- a/shared/io.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include <stdio.h> - -#include "bool.h" - -#pragma pack(push, 1) - -/** momentary button input struct */ -typedef struct { - bool pressed; -} w2_s_i_push; - -/** qtr contrast sensor input struct */ -typedef struct { - uint16_t range; -} w2_s_i_contrast; - -/** distance sensor input struct */ -typedef struct { - uint16_t detection; -} w2_s_i_distance; - -/** battery input struct */ -typedef struct { - uint16_t charge_level; -} w2_s_i_battery; - -/** motor output struct */ -typedef struct { - int16_t speed; -} w2_s_o_motor; - -/** underside led output struct */ -typedef struct { - bool on; -} w2_s_o_led; - -/** lcd output struct */ -typedef struct { - char text[16]; -} w2_s_o_display; - -/** struct containing all i/o */ -typedef struct { - w2_s_i_push button[5]; - w2_s_i_contrast qtr[5]; - w2_s_i_distance front_distance; - w2_s_i_distance side_distance; - w2_s_i_battery battery; - - w2_s_o_motor motor_left; - w2_s_o_motor motor_right; - w2_s_o_led led_red; - w2_s_o_led led_green; - w2_s_o_display lcd; -} w2_s_io_all; - -#pragma pack(pop) diff --git a/shared/protocol.c b/shared/protocol.c index 9be1d31..02d746a 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -18,8 +18,6 @@ void w2_cmd_setup_handlers() { g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_TX] = w2_cmd_bomd_tx; g_w2_cmd_handlers[W2_CMD_SRES | W2_CMDDIR_RX] = w2_cmd_sres_rx; g_w2_cmd_handlers[W2_CMD_MCFG | W2_CMDDIR_RX] = w2_cmd_mcfg_rx; - g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_RX] = w2_cmd_sens_rx; - g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_TX] = w2_cmd_sens_tx; g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_RX] = w2_cmd_info_rx; g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_TX] = w2_cmd_info_tx; g_w2_cmd_handlers[W2_CMD_DISP | W2_CMDDIR_RX] = w2_cmd_disp_rx; @@ -46,9 +44,6 @@ size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_leng if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sres_rx); - if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sens_rx); - if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_sens_tx); - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_info_rx); if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_info_tx); diff --git a/shared/protocol.h b/shared/protocol.h index 02d5526..5e1a12b 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -8,7 +8,6 @@ #include "bin.h" #include "bool.h" #include "consts.h" -#include "io.h" #define W2_SERIAL_START_BYTE 0xff @@ -136,15 +135,6 @@ typedef struct { typedef struct { uint8_t opcode; -} w2_s_cmd_sens_rx; - -typedef struct { - uint8_t opcode; - w2_s_io_all io; -} w2_s_cmd_sens_tx; - -typedef struct { - uint8_t opcode; } w2_s_cmd_info_rx; typedef struct { @@ -205,10 +195,6 @@ void w2_cmd_bomd_tx(w2_s_bin *data); void w2_cmd_sres_rx(w2_s_bin *data); /** handler for mcfg_rx (on complete message) */ void w2_cmd_mcfg_rx(w2_s_bin *data); -/** handler for sens_rx (on complete message) */ -void w2_cmd_sens_rx(w2_s_bin *data); -/** handler for sens_tx (on complete message) */ -void w2_cmd_sens_tx(w2_s_bin *data); /** handler for info_rx (on complete message) */ void w2_cmd_info_rx(w2_s_bin *data); /** handler for info_tx (on complete message) */ |