implement battery measurement and target area switching

5 files changed, 47 insertions, 13 deletions

diff --git a/robot/errcatch.c b/robot/errcatch.c
index 6a3452c..27026b5 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -46,6 +46,8 @@ void w2_errcatch_handle_error(w2_s_error *error) {
 			break;
 		case W2_E_WARN_PING_TIMEOUT:
 			break;
+		case W2_E_WARN_BATTERY_LOW:
+			break;
 		default: {
 			g_w2_error_uncaught = true;
 #ifdef W2_SIM
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index ba58977..ad7d493 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -8,10 +8,11 @@
 #include "../shared/protocol.h"
 
 /** amount of parallel timers */
-#define W2_HYPERVISOR_TIMER_COUNT 2
+#define W2_HYPERVISOR_TIMER_COUNT 3
 
 #define W2_TIMER_PING 0
 #define W2_TIMER_OBJECT_DETECTION 1
+#define W2_TIMER_BATTERY_MEASUREMENT 2
 
 extern uint64_t g_w2_hypervisor_cycles;
 extern uint64_t g_w2_hypervisor_uptime_qs;
diff --git a/robot/io.c b/robot/io.c
index 560b66f..8add1e9 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -1,11 +1,16 @@
-#include "io.h"
+#include <stdio.h>
+
 #include "../shared/consts.h"
 #include "../shared/errcatch.h"
+#include "../shared/util.h"
 #include "hypervisor.h"
+#include "io.h"
 #include "modes.h"
 #include "orangutan_shim.h"
 
-bool g_w2_io_object_detected = false;
+uint16_t g_w2_io_battery_mv		   = 0;
+uint8_t g_w2_io_battery_percentage = 0;
+bool g_w2_io_object_detected	   = false;
 
 void w2_io_object_detection() {
 	unsigned int front_distance = analog_read(W2_SIDE_SENSOR_PIN);
@@ -27,9 +32,32 @@ void w2_io_object_detection() {
 	}
 }
 
+void w2_io_battery_logic() {
+	if (w2_hypervisor_time_end(W2_TIMER_BATTERY_MEASUREMENT) <= W2_BATTERY_MEAS_FREQ) return;
+	w2_hypervisor_time_start(W2_TIMER_BATTERY_MEASUREMENT);
+
+	g_w2_io_battery_mv = 0;
+	for (int i = 0; i < W2_BATTERY_SAMPLES; i++)
+		g_w2_io_battery_mv += read_battery_millivolts() / W2_BATTERY_SAMPLES;
+	g_w2_io_battery_percentage = W2_RANGE(
+		0, (g_w2_io_battery_mv - W2_BATTERY_EMPTY) * 100 / (W2_BATTERY_FULL - W2_BATTERY_EMPTY),
+		100);
+
+	char battery_percent[9];
+	sprintf(battery_percent, "%i%%  ", g_w2_io_battery_percentage);
+	lcd_goto_xy(0, 1);
+	print(battery_percent);
+
+	if (g_w2_io_battery_percentage <= W2_BATTERY_PERCENTAGE_LOW &&
+		g_w2_target_area != W2_AREA_CHRG) {
+		w2_errcatch_throw(W2_E_WARN_BATTERY_LOW);
+		g_w2_target_area = W2_AREA_CHRG;
+	}
+}
+
 void w2_io_main() {
 	w2_io_object_detection();
-	// TODO: battery status
+	w2_io_battery_logic();
 
 	return;
 }
diff --git a/robot/io.h b/robot/io.h
index 31fe410..5e469bb 100644
--- a/robot/io.h
+++ b/robot/io.h
@@ -1,9 +1,11 @@
 #pragma once
 
-#include "../shared/bool.h"
-
 /** @file io.h */
 
+#include "../shared/bool.h"
+
+extern uint16_t g_w2_io_battery_mv;
+extern uint8_t g_w2_io_battery_percentage;
 extern bool g_w2_io_object_detected;
 
 /** @brief i/o module main */
diff --git a/robot/sercomm.c b/robot/sercomm.c
index a04d35f..0f251d2 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -154,13 +154,14 @@ 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_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());
+	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(g_w2_io_battery_mv);
+	res_msg->battery_percent = g_w2_io_battery_percentage;
 
 	w2_sercomm_append_msg(res_bin);
 	free(res_bin);