From 2a0270f3ba6eb993fb39ed3564f626d724156654 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 26 Jun 2022 17:40:36 +0200 Subject: implement battery measurement and target area switching --- client/main.c | 5 +++++ client/main.h | 1 - client/time.h | 1 + client/ui.c | 2 +- protocol.md | 6 ++++-- robot/errcatch.c | 2 ++ robot/hypervisor.h | 3 ++- robot/io.c | 34 +++++++++++++++++++++++++++++++--- robot/io.h | 6 ++++-- robot/sercomm.c | 15 ++++++++------- shared/consts.h | 11 +++++++++++ shared/protocol.h | 1 + 12 files changed, 70 insertions(+), 17 deletions(-) diff --git a/client/main.c b/client/main.c index 4d7f484..4eed232 100644 --- a/client/main.c +++ b/client/main.c @@ -33,5 +33,10 @@ int main(int argc, char **argv) { g_w2_state.ping_received = false; w2_send_ping(); } + + if (w2_timer_end(W2_TIMER_INFO) > W2_BATTERY_MEAS_FREQ) { + w2_send_info(); + w2_timer_start(W2_TIMER_INFO); + } } } diff --git a/client/main.h b/client/main.h index b72b507..e215ae6 100644 --- a/client/main.h +++ b/client/main.h @@ -9,7 +9,6 @@ typedef struct { bool ping_timeout; bool connected; - uint8_t battery_level; uint8_t mode; diff --git a/client/time.h b/client/time.h index a989c5c..130f91a 100644 --- a/client/time.h +++ b/client/time.h @@ -6,6 +6,7 @@ extern unsigned long g_w2_client_timers[W2_CLIENT_TIMER_COUNT]; typedef enum { W2_TIMER_PING = 0, W2_TIMER_UPDATE = 1, + W2_TIMER_INFO = 2, } w2_e_client_timers; void w2_timer_start(w2_e_client_timers label); diff --git a/client/ui.c b/client/ui.c index 6bc0986..61f9d0e 100644 --- a/client/ui.c +++ b/client/ui.c @@ -87,7 +87,7 @@ void w2_ui_paint_statusbar() { sprintf(temp, "(%s)", g_w2_state.info.build_str); w2_wmvaddstr(g_w2_ui_pad_statusbar, 0, g_w2_ui_width / 2 - strlen(temp) / 2, temp); - sprintf(temp, "%s %imv", W2_UI_BATT_STAT_BATTERY, g_w2_state.info.battery_mv); + sprintf(temp, "%s %i%%", W2_UI_BATT_STAT_BATTERY, g_w2_state.info.battery_percent); w2_wmvaddstr(g_w2_ui_pad_statusbar, 0, g_w2_ui_width - strlen(temp), temp); sprintf(temp, "[%s]", g_w2_mode_strings[g_w2_state.mode]); diff --git a/protocol.md b/protocol.md index 4139a22..9bb5a76 100644 --- a/protocol.md +++ b/protocol.md @@ -299,7 +299,7 @@ packet. requests robot info -#### robot info response (`r --> c`) (42 bytes) +#### robot info response (`r --> c`) (45 bytes) |type|description| |-:|-| @@ -311,6 +311,8 @@ requests robot info |`uint8_t`|exponential moving average modes module cycle time (ms)| |`uint32_t`|total robot uptime (s)| |`uint8_t`|current mode code| +|`uint16_t`|battery level (millivolts)| +|`uint8_t`|battery level (percentage)| robot info response @@ -324,7 +326,7 @@ robot info response ### TARQ -#### set target area (`r <-- c`) (1 byte) +#### set target area (`r <-- c`) (2 bytes) |type|description| |-:|-| 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 + #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); diff --git a/shared/consts.h b/shared/consts.h index 9c2358f..b6be4a4 100644 --- a/shared/consts.h +++ b/shared/consts.h @@ -52,3 +52,14 @@ #define W2_BATTERY_PIN 6 /** side-facing distance sensor pinout */ #define W2_SIDE_SENSOR_PIN 7 + +/** battery voltage measurement sample count */ +#define W2_BATTERY_SAMPLES 10 +/** battery full voltage (millivolts) */ +#define W2_BATTERY_FULL 4500 +/** battery empty voltage (millivolts) */ +#define W2_BATTERY_EMPTY 3300 +/** battery measurement interval (milliseconds) */ +#define W2_BATTERY_MEAS_FREQ 10e3 +/** battery low level percentage (target charging station) */ +#define W2_BATTERY_PERCENTAGE_LOW 30 diff --git a/shared/protocol.h b/shared/protocol.h index c13e56c..83d9111 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -155,6 +155,7 @@ typedef struct { uint32_t uptime_s; uint8_t mode; uint16_t battery_mv; + uint8_t battery_percent; } w2_s_cmd_info_tx; typedef struct { -- cgit v1.2.3