implemented obstacle detection

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);
+
diff --git a/robot/io.c b/robot/io.c
index 8b34aba..9e0eae1 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -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;
+}
diff --git a/robot/io.h b/robot/io.h
index 64ce293..14ffb38 100644
--- a/robot/io.h
+++ b/robot/io.h
@@ -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) */