fix object detection

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` */
diff --git a/robot/io.c b/robot/io.c
index 717778a..42c3a4b 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -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());