From 26ea92e552b6a15b999572448fafb1454be691b6 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 12 Apr 2026 19:43:27 -0600 Subject: [PATCH 01/41] Inital CLI implmenetation --- src/hello_robot.cpp | 46 ++++++++++++++++++++++++++++++++++++++++++++- src/hello_robot.hpp | 7 +++++++ 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 488512138..62eb1871f 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -56,6 +56,8 @@ void HelloRobot::run(){ update_controls(); check_safety(); loop_timing(); + + process_cli(); } } void HelloRobot::crash_report(){ @@ -204,7 +206,7 @@ void HelloRobot::check_safety(){ void HelloRobot::loop_timing(){ // print loopc every second to verify it is still alive if (loopc % 1000 == 0) { - Serial.println(loopc); + //Serial.println(loopc); } // LED heartbeat -- linked to loop count to reveal slowdowns and // freezes. @@ -217,4 +219,46 @@ void HelloRobot::loop_timing(){ // Keep the loop running at the desired rate loop_timer.delay_micros((int)(1E6 / (float)(LOOP_FREQ))); } +void HelloRobot::process_cli(){ +// Read all available characters in the hardware buffer + while (Serial.available() > 0) { + char c = Serial.read(); + + // If the character is a newline or carriage return, the command is complete + if (c == '\n' || c == '\r') { + if (cli_index == 0) continue; // Ignore empty lines + cli_buffer[cli_index] = '\0'; // Null-terminate the string + String cmd = String(cli_buffer); + cmd.trim(); + // ========================================== + // COMMAND DICTIONARY + // ========================================== + if (cmd == "ping") { + Serial.println("pong! Robot is alive."); + } + else if (cmd == "print tx") { + // Access the transmitter data + Serial.printf(" \rSafety: %d | L_X: %.2f | L_Y: %.2f | R_X: %.2f | R_Y: %.2f", + transmitter_manager.transmitter.is_safety_mode(), + transmitter_manager.get_l_stick_x(), + transmitter_manager.get_l_stick_y(), + transmitter_manager.get_r_stick_x(), + transmitter_manager.get_r_stick_y()); + } + else if (cmd == "print dt") { + Serial.printf("Last loop dt: %f seconds\n", stall_timer.delta()); + } + else { + Serial.println("Unknown command. Try: ping"); + } + + // Reset the buffer for the next command + cli_index = 0; + } + else if (cli_index < 63) { + // Add the typed character to the buffer + cli_buffer[cli_index++] = c; + } + } +} diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 271b22436..26e6ce5ff 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -77,6 +77,11 @@ class HelloRobot { std::optional hive_state_map_offset;// Hive offset state bool override_request = false; + // CLI Buffer variables + char cli_buffer[64] = {0}; + uint8_t cli_index = 0; + + void setup(); void crash_report(); void read_telemetry(); @@ -84,6 +89,8 @@ class HelloRobot { void update_controls(); void check_safety(); void loop_timing(); + + void process_cli(); public: void init(); From 12afef10db4d84242a5666afd722e83b7873f97d Mon Sep 17 00:00:00 2001 From: EricV Date: Thu, 16 Apr 2026 21:08:42 -0600 Subject: [PATCH 02/41] ping works on standalone teensy --- src/hello_robot.cpp | 16 +++++++++------- src/main.cpp | 3 +-- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 62eb1871f..34a56d83e 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -55,11 +55,13 @@ void HelloRobot::run(){ process_behaviors(); update_controls(); check_safety(); + process_cli(); loop_timing(); - process_cli(); + } } + void HelloRobot::crash_report(){ // check to see if there is a crash report, and if so, print it repeatedly // over Serial in the future, we'll send this directly over comms @@ -219,11 +221,11 @@ void HelloRobot::loop_timing(){ // Keep the loop running at the desired rate loop_timer.delay_micros((int)(1E6 / (float)(LOOP_FREQ))); } + void HelloRobot::process_cli(){ -// Read all available characters in the hardware buffer + // Read all available characters in the hardware buffer while (Serial.available() > 0) { char c = Serial.read(); - // If the character is a newline or carriage return, the command is complete if (c == '\n' || c == '\r') { if (cli_index == 0) continue; // Ignore empty lines @@ -245,13 +247,13 @@ void HelloRobot::process_cli(){ transmitter_manager.get_l_stick_y(), transmitter_manager.get_r_stick_x(), transmitter_manager.get_r_stick_y()); - } - else if (cmd == "print dt") { - Serial.printf("Last loop dt: %f seconds\n", stall_timer.delta()); } + else if (cmd == "print dt") { + Serial.printf("Last loop dt: %f seconds\n", stall_timer.delta()); + } else { Serial.println("Unknown command. Try: ping"); - } + } // Reset the buffer for the next command cli_index = 0; diff --git a/src/main.cpp b/src/main.cpp index f613a756a..afec5e33e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,13 +1,12 @@ #include "hello_robot.hpp" -//extern "C" void reset_teensy(void); - // Master loop int main() { Serial.begin(115200); // the serial monitor is actually always active (for // debug use Serial.println & tycmd) + while(!Serial); debug.begin(SerialUSB1); //Print Splash Screen From 497625b77fbe23fcf0624d81fb39892d086a5d7e Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 15:01:37 -0600 Subject: [PATCH 03/41] added CLI implementation --- Makefile | 3 +- src/hello_robot.cpp | 101 ++++++++++++++++++++++++++++++++++++----- src/hello_robot.hpp | 8 ++-- src/utils/profiler.cpp | 34 ++++++++++++++ src/utils/profiler.hpp | 6 ++- 5 files changed, 134 insertions(+), 18 deletions(-) diff --git a/Makefile b/Makefile index 982f0bc5e..37ac75067 100644 --- a/Makefile +++ b/Makefile @@ -220,7 +220,8 @@ upload: build # Teensy serial isn't immediately available after upload, so we wait a bit # The Teensy waits for 20 + 280 + 20 ms after power up/boot @sleep 0.4s - @bash $(TOOLS_DIR)/monitor.sh +#@bash $(TOOLS_DIR)/monitor.sh + @tycmd monitor # Install required tools for building and uploading firmware diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 34a56d83e..deb8bd271 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -50,12 +50,33 @@ void HelloRobot::run(){ while (true) { // start main loop time timer stall_timer.start(); - + + #ifdef PROFILER + prof.begin("Telemetry"); + read_telemetry(); + prof.end("Telemetry"); + + prof.begin("Behaviors"); + process_behaviors(); + prof.end("Behaviors"); + + prof.begin("Controls"); + update_controls(); + prof.end("Controls"); + + prof.begin("Safety"); + check_safety(); + prof.end("Safety"); + prof.begin("CLI"); + process_cli(); + prof.end("CLI"); + #else read_telemetry(); process_behaviors(); update_controls(); check_safety(); process_cli(); + #endif loop_timing(); @@ -221,8 +242,35 @@ void HelloRobot::loop_timing(){ // Keep the loop running at the desired rate loop_timer.delay_micros((int)(1E6 / (float)(LOOP_FREQ))); } - void HelloRobot::process_cli(){ + + static bool live_profiler_active = false; + static uint32_t last_redraw_time = 0; + + // Live view mode is for screen refreshing to make it look like a dashboard + if (live_profiler_active) { + // Redraw the screen only once every 1000 milliseconds + if (millis() - last_redraw_time >= 1000) { + Serial.print("\033[H"); // Move cursor to top-left + prof.print_summary(); + Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); + last_redraw_time = millis(); + } + + // If the user types anything, exit live mode + if (Serial.available() > 0) { + live_profiler_active = false; + + // Flush the buffer so the exit keystroke isn't read as a command + while(Serial.available()) Serial.read(); + + Serial.println("\n\n[Exited Live Profiler]"); + cli_index = 0; + } + + return; + } + // Read all available characters in the hardware buffer while (Serial.available() > 0) { char c = Serial.read(); @@ -239,18 +287,47 @@ void HelloRobot::process_cli(){ if (cmd == "ping") { Serial.println("pong! Robot is alive."); } - else if (cmd == "print tx") { - // Access the transmitter data - Serial.printf(" \rSafety: %d | L_X: %.2f | L_Y: %.2f | R_X: %.2f | R_Y: %.2f", - transmitter_manager.transmitter.is_safety_mode(), - transmitter_manager.get_l_stick_x(), - transmitter_manager.get_l_stick_y(), - transmitter_manager.get_r_stick_x(), - transmitter_manager.get_r_stick_y()); - } + /*else if (cmd == "print tx") { + // Access the transmitter data + Serial.printf(" \rSafety: %d | L_X: %.2f | L_Y: %.2f | R_X: %.2f | R_Y: %.2f", + transmitter_manager.transmitter.is_safety_mode(), + transmitter_manager.get_l_stick_x(), + transmitter_manager.get_l_stick_y(), + transmitter_manager.get_r_stick_x(), + transmitter_manager.get_r_stick_y()); + }*/ else if (cmd == "print dt") { Serial.printf("Last loop dt: %f seconds\n", stall_timer.delta()); - } + } + else if (cmd == "prof") { + live_profiler_active = true; + last_redraw_time = 0; // Force an immediate redraw + Serial.print("\033[2J"); // Clear the terminal screen + } + else if (cmd == "print estimated state"){ + stimated_state_map.print(); + } + else if (cmd== "heartbeat"){ + // Redraw the screen only once every 1000 milliseconds + if (millis() - last_redraw_time >= 1000) { + Serial.print("\033[H"); // Move cursor to top-left + Serial.println(loopc); + Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); + last_redraw_time = millis(); + } + + // If the user types anything, exit live mode + if (Serial.available() > 0) { + + // Flush the buffer so the exit keystroke isn't read as a command + while(Serial.available()) Serial.read(); + + Serial.println("\n\n[Exited Live Profiler]"); + cli_index = 0; + } + + return; + } else { Serial.println("Unknown command. Try: ping"); } diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 26e6ce5ff..701ee98d9 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -38,11 +38,12 @@ extern "C" void reset_teensy(void); // Loop constants #define LOOP_FREQ 1000 #define HEARTBEAT_FREQ 2 -#ifdef PROFILER -Profiler prof; -#endif + class HelloRobot { private: +#ifdef PROFILER + Profiler prof; +#endif CANManager can; //RefSystem ref; TransmitterManager transmitter_manager; @@ -80,6 +81,7 @@ class HelloRobot { // CLI Buffer variables char cli_buffer[64] = {0}; uint8_t cli_index = 0; + bool live_profiler_active = false; void setup(); diff --git a/src/utils/profiler.cpp b/src/utils/profiler.cpp index 1195c8b39..1ecc827e4 100644 --- a/src/utils/profiler.cpp +++ b/src/utils/profiler.cpp @@ -83,3 +83,37 @@ void Profiler::print(const char *name) { } #endif } +void Profiler::print_summary() { +#ifdef PROFILER + Serial.println("\n================ PROFILER SUMMARY ================"); + Serial.println(" Subsystem | Avg Time (us) | Max Time (us) "); + Serial.println("--------------------------------------------------"); + + // Iterate through all possible sections + for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) { + // If the section actually has a name, it's active + if (sections[i].name[0] != '\0') { + uint32_t sum = 0; + uint32_t max = 0; + + uint32_t trueCount = sections[i].overflowed ? PROF_MAX_TIMES : sections[i].count; + if (trueCount == 0) continue; // Skip if no data yet + + for (uint32_t j = 0; j < trueCount; j++) { + // if the last run was started and not ended, ignore it + if (sections[i].started && j == sections[i].count) continue; + + uint32_t delta = sections[i].time_lengths[j]; + sum += delta; + if (delta > max) max = delta; + } + + uint32_t avg = sum / trueCount; + + // %-16s pads the string to 16 characters so the table columns align perfectly + Serial.printf(" %-16s | %13u | %13u \n", sections[i].name, avg, max); + } + } + Serial.println("==================================================\n"); +#endif +} diff --git a/src/utils/profiler.hpp b/src/utils/profiler.hpp index a7caafb1e..128f0b9e9 100644 --- a/src/utils/profiler.hpp +++ b/src/utils/profiler.hpp @@ -2,11 +2,11 @@ #define PROFILER_H // Use this flag to toggle profiling globally. -// #define PROFILER +#define PROFILER #include -#define PROF_MAX_SECTIONS 4 // max number of active profiling sections +#define PROF_MAX_SECTIONS 10 // max number of active profiling sections #define PROF_MAX_NAME 16 // max length of section name #define PROF_MAX_TIMES (1000) // max number of start/end times per section @@ -45,6 +45,8 @@ struct Profiler { /// @brief Print stats for a profiling section /// @param name The name of the section to print stats for void print(const char *name); + + void print_summary(); }; extern Profiler prof; // Global profiler From e9128015b09e8eee7f50eead573a40afef98de28 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 17:42:33 -0600 Subject: [PATCH 04/41] idk --- src/hello_robot.cpp | 114 +++++++++++++++++++++++++++----------------- 1 file changed, 69 insertions(+), 45 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index deb8bd271..46ac0e114 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -244,33 +244,64 @@ void HelloRobot::loop_timing(){ } void HelloRobot::process_cli(){ - static bool live_profiler_active = false; static uint32_t last_redraw_time = 0; - - // Live view mode is for screen refreshing to make it look like a dashboard - if (live_profiler_active) { - // Redraw the screen only once every 1000 milliseconds - if (millis() - last_redraw_time >= 1000) { + static uint32_t redraw_interval = 1000; // Time in ms between frames + enum class LiveMode { NONE, PROFILER_VIEW, TRANSMITTER, ESTIMATED_STATE, HEARTBEAT }; + + static LiveMode current_live_mode = LiveMode::NONE; + + // ========================================== + // 2. LIVE VIEW RENDERER + // ========================================== + if (current_live_mode != LiveMode::NONE) { + + // Check if it's time to draw the next frame + if (millis() - last_redraw_time >= redraw_interval) { Serial.print("\033[H"); // Move cursor to top-left - prof.print_summary(); + + // Draw whichever screen is currently active + switch (current_live_mode) { + case LiveMode::PROFILER_VIEW: + prof.print_summary(); + break; + + case LiveMode::TRANSMITTER: + Serial.printf("=== LIVE TRANSMITTER DATA ===\n"); + //Serial.printf(" Safety Mode: %s\n", transmitter_manager.is_safety_mode() ? "ON" : "OFF"); + //Serial.printf(" Left Stick: X: %5.2f | Y: %5.2f\n", transmitter.get_l_stick_x(), transmitter.get_l_stick_y()); + //Serial.printf(" Right Stick: X: %5.2f | Y: %5.2f\n", transmitter.get_r_stick_x(), transmitter.get_r_stick_y()); + break; + case LiveMode::ESTIMATED_STATE: + Serial.printf("=== LIVE ESTIMATED STATE ===\n"); + estimated_state_map->print(); + break; + + case LiveMode::HEARTBEAT: + Serial.printf("=== LIVE HEARTBEAT ===\n"); + Serial.println(loopc); + break; + + default: + break; + } + Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); last_redraw_time = millis(); } // If the user types anything, exit live mode if (Serial.available() > 0) { - live_profiler_active = false; + current_live_mode = LiveMode::NONE; - // Flush the buffer so the exit keystroke isn't read as a command + // Flush the buffer while(Serial.available()) Serial.read(); - Serial.println("\n\n[Exited Live Profiler]"); + Serial.println("\n\n[Exited Live View]"); cli_index = 0; } - return; + return; // Return immediately so normal parsing doesn't run } - // Read all available characters in the hardware buffer while (Serial.available() > 0) { char c = Serial.read(); @@ -281,55 +312,48 @@ void HelloRobot::process_cli(){ cli_buffer[cli_index] = '\0'; // Null-terminate the string String cmd = String(cli_buffer); cmd.trim(); + // ========================================== // COMMAND DICTIONARY // ========================================== + /* Adding new commands is simple. + add if statement to cmd elseif block below + If it is live add to live mode enum and switch statement above + as well as to cmd elseif block below + */ if (cmd == "ping") { Serial.println("pong! Robot is alive."); } - /*else if (cmd == "print tx") { - // Access the transmitter data - Serial.printf(" \rSafety: %d | L_X: %.2f | L_Y: %.2f | R_X: %.2f | R_Y: %.2f", - transmitter_manager.transmitter.is_safety_mode(), - transmitter_manager.get_l_stick_x(), - transmitter_manager.get_l_stick_y(), - transmitter_manager.get_r_stick_x(), - transmitter_manager.get_r_stick_y()); - }*/ + else if (cmd == "print tx") { + current_live_mode = LiveMode::TRANSMITTER; + redraw_interval = 100; // 10Hz + last_redraw_time = 0; + Serial.print("\033[2J"); + } else if (cmd == "print dt") { Serial.printf("Last loop dt: %f seconds\n", stall_timer.delta()); } else if (cmd == "prof") { - live_profiler_active = true; - last_redraw_time = 0; // Force an immediate redraw - Serial.print("\033[2J"); // Clear the terminal screen + current_live_mode = LiveMode::PROFILER_VIEW; + redraw_interval = 1000; // 1Hz + last_redraw_time = 0; // Force immediate redraw + Serial.print("\033[2J"); // Clear screen } else if (cmd == "print estimated state"){ - stimated_state_map.print(); + + current_live_mode = LiveMode::ESTIMATED_STATE; + redraw_interval = 500; // 2Hz update + last_redraw_time = 0; + Serial.print("\033[2J"); } else if (cmd== "heartbeat"){ - // Redraw the screen only once every 1000 milliseconds - if (millis() - last_redraw_time >= 1000) { - Serial.print("\033[H"); // Move cursor to top-left - Serial.println(loopc); - Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); - last_redraw_time = millis(); - } - - // If the user types anything, exit live mode - if (Serial.available() > 0) { - - // Flush the buffer so the exit keystroke isn't read as a command - while(Serial.available()) Serial.read(); - - Serial.println("\n\n[Exited Live Profiler]"); - cli_index = 0; - } - - return; + current_live_mode = LiveMode::HEARTBEAT; + redraw_interval = 500; // 2Hz update + last_redraw_time = 0; + Serial.print("\033[2J"); } else { - Serial.println("Unknown command. Try: ping"); + Serial.println("Unknown command. Try: ping, print dt, prof, print estimated state, heartbeat"); } // Reset the buffer for the next command From 11e352f3a04ec86c53de14fe43983a214b3e3e08 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 18:47:46 -0600 Subject: [PATCH 05/41] finished merging with gerald... we are one now --- src/hello_robot.cpp | 6 +++--- src/hello_robot.hpp | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 9eac1c9c0..dcc7319fe 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -126,7 +126,7 @@ void HelloRobot::read_telemetry(){ } void HelloRobot::process_behaviors(){ // manual controls on firmware - transmitter_manager.manual_controls(*estimated_state_map, *target_state_map, not_safety_mode, feed, last_feed); + transmitter_manager.manual_controls(*estimated_state_map, *target_state_map, not_safety_mode, feed, last_feed, has_lower_feeder); // check if we want to use hive controls instead if (transmitter_manager.is_hive_mode()) { @@ -225,7 +225,8 @@ void HelloRobot::check_safety(){ // SAFETY ON // TODO: Reset all controller integrators here can.issue_safety_mode(); - governor->set_position_reference(Cfg::StateName::Feeder, (*estimated_state_map)[Cfg::StateName::Feeder].get_position()); +if (has_lower_feeder) governor->set_position_reference(Cfg::StateName::LowerFeeder, (*estimated_state_map)[Cfg::StateName::LowerFeeder].get_position()); + float current_feed = (*estimated_state_map)[Cfg::StateName::Feeder].get_position(); feed = (fmod(fmod(current_feed, 1) + 1, 1) > 0.2) ? (int)floor(current_feed) + 1 @@ -233,7 +234,6 @@ void HelloRobot::check_safety(){ last_feed = feed; // reset last feed to the current state //Serial.printf("Can zero\n"); } - } void HelloRobot::loop_timing(){ // print loopc every second to verify it is still alive diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 701ee98d9..e477ca749 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -70,6 +70,7 @@ class HelloRobot { // manual controls variables float feed = 0; float last_feed = 0; + bool has_lower_feeder = false; // variables for use in loop std::optional governor; std::optional estimated_state_map; From 612612ea2b7bacd0f3948e52050ba95c1cd9f887 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 19:51:46 -0600 Subject: [PATCH 06/41] changed makefile to use tycmd monitor --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index b53e5735b..fe75c9f84 100644 --- a/Makefile +++ b/Makefile @@ -243,7 +243,7 @@ gdb: # monitors currently running firmware on robot monitor: @echo [Monitoring] - @bash $(TOOLS_DIR)/monitor.sh + @tycmd monitor # resets teensy and switches it into boot-loader mode, effectively stopping any execution From 695eaf41067e3fada2424ddb2681bf0d5450b599 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 22:56:29 -0600 Subject: [PATCH 07/41] adjusted profiler for performance and added print options to every sensor --- src/hello_robot.cpp | 36 +++++-- src/sensors/AdafruitIMUSensor.cpp | 16 +++- src/sensors/AdafruitIMUSensor.hpp | 4 +- src/sensors/StereoCamTrigger.cpp | 7 +- src/sensors/StereoCamTrigger.hpp | 6 +- src/sensors/buff_encoder.cpp | 7 +- src/sensors/buff_encoder.hpp | 2 + src/sensors/d200.cpp | 6 +- src/sensors/d200.hpp | 5 +- src/sensors/limit_switch.cpp | 4 + src/sensors/limit_switch.hpp | 2 + src/sensors/rev_encoder.cpp | 6 +- src/sensors/rev_encoder.hpp | 4 +- src/sensors/sensor.hpp | 5 +- src/sensors/sensor_manager.cpp | 7 +- src/sensors/sensor_manager.hpp | 5 +- src/sensors/transmitter/ET16S.cpp | 12 +++ src/sensors/transmitter/ET16S.hpp | 2 + src/sensors/transmitter/dr16.cpp | 18 ++++ src/sensors/transmitter/dr16.hpp | 2 + src/sensors/transmitter/transmitter.hpp | 2 + .../transmitter/transmitter_manager.cpp | 8 ++ .../transmitter/transmitter_manager.hpp | 4 +- src/utils/profiler.cpp | 95 +++++-------------- src/utils/profiler.hpp | 12 +-- 25 files changed, 177 insertions(+), 100 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index dcc7319fe..0183c7cb7 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -255,12 +255,12 @@ void HelloRobot::process_cli(){ static uint32_t last_redraw_time = 0; static uint32_t redraw_interval = 1000; // Time in ms between frames - enum class LiveMode { NONE, PROFILER_VIEW, TRANSMITTER, ESTIMATED_STATE, HEARTBEAT }; + enum class LiveMode { NONE, PROFILER_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE,HEARTBEAT ,SENSORS}; static LiveMode current_live_mode = LiveMode::NONE; // ========================================== - // 2. LIVE VIEW RENDERER + // LIVE VIEW // ========================================== if (current_live_mode != LiveMode::NONE) { @@ -273,23 +273,28 @@ void HelloRobot::process_cli(){ case LiveMode::PROFILER_VIEW: prof.print_summary(); break; - + case LiveMode::TRANSMITTER: - Serial.printf("=== LIVE TRANSMITTER DATA ===\n"); - //Serial.printf(" Safety Mode: %s\n", transmitter_manager.is_safety_mode() ? "ON" : "OFF"); - //Serial.printf(" Left Stick: X: %5.2f | Y: %5.2f\n", transmitter.get_l_stick_x(), transmitter.get_l_stick_y()); - //Serial.printf(" Right Stick: X: %5.2f | Y: %5.2f\n", transmitter.get_r_stick_x(), transmitter.get_r_stick_y()); + transmitter_manager.print_live_data(); break; + case LiveMode::ESTIMATED_STATE: Serial.printf("=== LIVE ESTIMATED STATE ===\n"); estimated_state_map->print(); break; - + + case LiveMode::TARGET_STATE: + Serial.printf("=== LIVE TARGET STATE ===\n"); + target_state_map->print(); + break; + case LiveMode::HEARTBEAT: Serial.printf("=== LIVE HEARTBEAT ===\n"); Serial.println(loopc); break; - + case LiveMode::SENSORS: + sensor_manager.print_sensors_live(); + break; default: break; } @@ -349,12 +354,23 @@ void HelloRobot::process_cli(){ Serial.print("\033[2J"); // Clear screen } else if (cmd == "print estimated state"){ - current_live_mode = LiveMode::ESTIMATED_STATE; redraw_interval = 500; // 2Hz update last_redraw_time = 0; Serial.print("\033[2J"); } + else if (cmd == "print target state"){ + current_live_mode = LiveMode::TARGET_STATE; + redraw_interval = 500; // 2Hz update + last_redraw_time = 0; + Serial.print("\033[2J"); + } + else if (cmd == "print sensors") { + current_live_mode = LiveMode::SENSORS; + redraw_interval = 100; + last_redraw_time = 0; + Serial.print("\033[2J"); + } else if (cmd== "heartbeat"){ current_live_mode = LiveMode::HEARTBEAT; redraw_interval = 500; // 2Hz update diff --git a/src/sensors/AdafruitIMUSensor.cpp b/src/sensors/AdafruitIMUSensor.cpp index e552a539f..250c58f14 100644 --- a/src/sensors/AdafruitIMUSensor.cpp +++ b/src/sensors/AdafruitIMUSensor.cpp @@ -23,4 +23,18 @@ void AdafruitIMUSensor::print() { Serial.print(get_gyro_Z()); Serial.println(" radians/s "); Serial.println(); -} \ No newline at end of file +} +void AdafruitIMUSensor::print_live_data() { + Serial.printf("=== LIVE ADAFRUIT IMU DATA ===\n"); + + // Print temperature + Serial.printf(" Temperature: %5.2f °C\n", get_temperature()); + Serial.println("------------------------------------------------"); + + // Print Acceleration and Gyro data + Serial.printf(" Accel (m/s^2) | X: %6.2f | Y: %6.2f | Z: %6.2f\n", + get_accel_X(), get_accel_Y(), get_accel_Z()); + + Serial.printf(" Gyro (rad/s) | X: %6.2f | Y: %6.2f | Z: %6.2f\n", + get_gyro_X(), get_gyro_Y(), get_gyro_Z()); +} diff --git a/src/sensors/AdafruitIMUSensor.hpp b/src/sensors/AdafruitIMUSensor.hpp index ef45bb415..e58120eba 100644 --- a/src/sensors/AdafruitIMUSensor.hpp +++ b/src/sensors/AdafruitIMUSensor.hpp @@ -53,6 +53,8 @@ class AdafruitIMUSensor : public Sensor { /// @brief Print out all IMU data to Serial for debugging purposes void print(); + /// @brief Prints a live, formatted dashboard of IMU data + void print_live_data() override; protected: // sensor events to read from @@ -91,4 +93,4 @@ class AdafruitIMUSensor : public Sensor { /// @brief temperature value float temperature = 0; -}; \ No newline at end of file +}; diff --git a/src/sensors/StereoCamTrigger.cpp b/src/sensors/StereoCamTrigger.cpp index d1481bb85..c021a961b 100644 --- a/src/sensors/StereoCamTrigger.cpp +++ b/src/sensors/StereoCamTrigger.cpp @@ -48,4 +48,9 @@ void StereoCamTrigger::send_to_comms() const { sendable.data = comms_data; sendable.send_to_comms(); -} \ No newline at end of file +} + +void StereoCamTrigger::print_live_data() { + Serial.printf(" [Stereo Cam] Status: %s | Last Exposure (us): %u\n", + stopped ? "STOPPED" : "RUNNING", latest_exposure_timestamp); +} diff --git a/src/sensors/StereoCamTrigger.hpp b/src/sensors/StereoCamTrigger.hpp index 0a04b29b3..b3ba2c45d 100644 --- a/src/sensors/StereoCamTrigger.hpp +++ b/src/sensors/StereoCamTrigger.hpp @@ -39,6 +39,9 @@ class StereoCamTrigger : public Sensor{ /// @brief Send exposure timestamp and estimated state at exposure to comms /// @note This is not implemented currently void send_to_comms() const override; + + /// @brief Prints a formatted dashboard of the camera trigger state + void print_live_data() override; /// @brief start interval timer. Begins sending trigger signal to cameras via timer interrupt /// @param res the desired resolution (or interval size) of the timer interrupt in micros @@ -46,4 +49,5 @@ class StereoCamTrigger : public Sensor{ /// @brief stop interval timer. Can start again by calling start() void stop(); -}; \ No newline at end of file + +}; diff --git a/src/sensors/buff_encoder.cpp b/src/sensors/buff_encoder.cpp index 28468ecd9..67e8d814f 100644 --- a/src/sensors/buff_encoder.cpp +++ b/src/sensors/buff_encoder.cpp @@ -46,4 +46,9 @@ void BuffEncoder::send_to_comms() const { void BuffEncoder::print() const{ Serial.printf("Buff Encoder:\n\t"); Serial.println(get_angle()); -} \ No newline at end of file +} +void BuffEncoder::print_live_data() { + // Note: casting get_name() to int so it prints the enum number + Serial.printf(" [Buff Encoder %d] Angle (rad): %8.4f\n", + (int)get_name(), get_angle()); +} diff --git a/src/sensors/buff_encoder.hpp b/src/sensors/buff_encoder.hpp index 8ddf6a21f..395a523fc 100644 --- a/src/sensors/buff_encoder.hpp +++ b/src/sensors/buff_encoder.hpp @@ -68,6 +68,8 @@ class BuffEncoder : public Sensor { /// @brief Print the data for debugging void print() const; + /// @brief Prints a formatted dashboard of live Buff Encoder values + void print_live_data() override; private: diff --git a/src/sensors/d200.cpp b/src/sensors/d200.cpp index 99aa95982..3f49a42a3 100644 --- a/src/sensors/d200.cpp +++ b/src/sensors/d200.cpp @@ -231,4 +231,8 @@ void D200LD14P::print_latest_packet() { Serial.println(p.end_angle); Serial.print("timestamp: "); Serial.println(p.timestamp); -} \ No newline at end of file +} +void D200LD14P::print_live_data() { + Serial.printf(" [D200 Lidar] Latest Pkt Index: %d | Yaw: %5.2f rad | Yaw Vel: %5.2f rad/s\n", + get_latest_packet_index(), robot_yaw, robot_yaw_velocity); +} diff --git a/src/sensors/d200.hpp b/src/sensors/d200.hpp index f46bd99f0..5ef2d719c 100644 --- a/src/sensors/d200.hpp +++ b/src/sensors/d200.hpp @@ -164,6 +164,9 @@ class D200LD14P : public Sensor { /// @brief send latest packet(s) to comms void send_to_comms() const override; + + /// @brief Prints a formatted dashboard of live Lidar stats + void print_live_data() override; /// @brief set rotation the speed of the LiDAR /// @param speed desired rotation speed of LiDAR (rad/s) @@ -209,4 +212,4 @@ class D200LD14P : public Sensor { /// @brief return the current packet /// @return the current packet index int get_current_packet_index() { return current_packet; }; -}; \ No newline at end of file +}; diff --git a/src/sensors/limit_switch.cpp b/src/sensors/limit_switch.cpp index ab5436293..879979e59 100644 --- a/src/sensors/limit_switch.cpp +++ b/src/sensors/limit_switch.cpp @@ -17,4 +17,8 @@ void LimitSwitch::send_to_comms() const { sendable.data = comms_data; sendable.send_to_comms(); } +void LimitSwitch::print_live_data() { + Serial.printf(" [Limit Switch %d] Status: %s\n", + (int)config.switch_name, get_is_pressed() ? "PRESSED (CLOSED)" : "OPEN"); +} diff --git a/src/sensors/limit_switch.hpp b/src/sensors/limit_switch.hpp index 213a6ba53..6a494ab2c 100644 --- a/src/sensors/limit_switch.hpp +++ b/src/sensors/limit_switch.hpp @@ -22,6 +22,8 @@ class LimitSwitch : public Sensor { /// @brief Get whether the limit switch is currently pressed /// @return true if the limit switch is pressed, false otherwise inline bool get_is_pressed() const { return is_pressed; } + /// @brief Prints a formatted dashboard of the live Limit Switch state + void print_live_data() override; private: /// @brief Configuration data for the limit switch diff --git a/src/sensors/rev_encoder.cpp b/src/sensors/rev_encoder.cpp index 53b939a3c..e4f5d534d 100644 --- a/src/sensors/rev_encoder.cpp +++ b/src/sensors/rev_encoder.cpp @@ -52,4 +52,8 @@ void RevEncoder::print() { Serial.println(ticks); Serial.print("\tRadians: "); Serial.println(radians); -} \ No newline at end of file +} +void RevEncoder::print_live_data() { + Serial.printf(" [Rev Encoder %d] Angle (rad): %8.4f | Ticks: %8.0f\n", + (int)config.encoder_name, get_angle_radians(), get_angle_ticks()); +} diff --git a/src/sensors/rev_encoder.hpp b/src/sensors/rev_encoder.hpp index 28a8a988e..b83de4ce8 100644 --- a/src/sensors/rev_encoder.hpp +++ b/src/sensors/rev_encoder.hpp @@ -41,4 +41,6 @@ class RevEncoder : public Sensor{ /// @brief print the encoder details void print(); -}; \ No newline at end of file + /// @brief Prints a formatted dashboard of live Rev Encoder values + void print_live_data() override; +}; diff --git a/src/sensors/sensor.hpp b/src/sensors/sensor.hpp index 5dc8dc175..f06236de2 100644 --- a/src/sensors/sensor.hpp +++ b/src/sensors/sensor.hpp @@ -11,8 +11,9 @@ virtual void init() = 0; /// @brief Read data from the sensor and update internal state accordingly. virtual void read() = 0; - +/// @brief Prints a formatted dashboard of live sensor values. Default does nothing. +virtual void print_live_data(){} /// @brief Send the current sensor data to the comms layer. virtual void send_to_comms() const = 0; -}; \ No newline at end of file +}; diff --git a/src/sensors/sensor_manager.cpp b/src/sensors/sensor_manager.cpp index 5ee5f7b09..fc035959a 100644 --- a/src/sensors/sensor_manager.cpp +++ b/src/sensors/sensor_manager.cpp @@ -76,4 +76,9 @@ void SensorManager::send_to_comms() { for(auto& [sensor_name, sensor] : sensors) { sensor->send_to_comms(); } -} \ No newline at end of file +} +void SensorManager::print_sensors_live() { + for(auto& [sensor_name, sensor] : sensors) { + sensor->print_live_data(); + } +} diff --git a/src/sensors/sensor_manager.hpp b/src/sensors/sensor_manager.hpp index c9be7cad5..4dfbc89c7 100644 --- a/src/sensors/sensor_manager.hpp +++ b/src/sensors/sensor_manager.hpp @@ -35,7 +35,8 @@ class SensorManager { void read(); /// @brief Call each sensor's send_to_comms function to send their data to comms void send_to_comms(); - + /// @brief Triggers the live dashboard for any supported sensors + void print_sensors_live(); /// @brief Get a sensor by its name and type. Will trigger safety procedure if the sensor is not found or is not of the requested type. /// @param name The name of the sensor to get /// @tparam SensorType The type of the sensor to get, must be derived from the Sensor class @@ -58,4 +59,4 @@ class SensorManager { private: /// @brief Map of sensor pointers by name std::map> sensors; -}; \ No newline at end of file +}; diff --git a/src/sensors/transmitter/ET16S.cpp b/src/sensors/transmitter/ET16S.cpp index ee51b6f0e..ad2c7a55c 100644 --- a/src/sensors/transmitter/ET16S.cpp +++ b/src/sensors/transmitter/ET16S.cpp @@ -106,6 +106,18 @@ void ET16S::print_raw_bin(uint8_t m_inputRaw[ET16S_PACKET_SIZE]) { Serial.println(); } +void ET16S::print_live_data() { + Serial.printf("=== LIVE ET16S TRANSMITTER DATA ===\n"); + Serial.printf(" Safety Mode: %s\n", is_safety_mode() ? "ON" : "OFF"); + Serial.printf(" Control Mode: %s\n", is_teensy_mode() ? "TEENSY" : (is_hive_mode() ? "HIVE" : "UNKNOWN")); + Serial.println("-----------------------------------"); + Serial.printf(" L Stick: X: %5.2f | Y: %5.2f\n", get_l_stick_x(), get_l_stick_y()); + Serial.printf(" R Stick: X: %5.2f | Y: %5.2f\n", get_r_stick_x(), get_r_stick_y()); + Serial.printf(" L Dial : %5.2f | R Dial : %5.2f\n", get_l_dial(), get_r_dial()); + Serial.println("-----------------------------------"); + Serial.printf(" SW_B: %d | SW_C: %d | SW_D: %d | SW_E: %d\n", + (int)get_switch_b(), (int)get_switch_c(), (int)get_switch_d(), (int)get_switch_e()); +} void ET16S::print_format_bin(int channel_num) { if (channel_num > ET16S_INPUT_VALUE_COUNT || channel_num < 0) { diff --git a/src/sensors/transmitter/ET16S.hpp b/src/sensors/transmitter/ET16S.hpp index ff232160d..800987ba2 100644 --- a/src/sensors/transmitter/ET16S.hpp +++ b/src/sensors/transmitter/ET16S.hpp @@ -90,6 +90,8 @@ class ET16S : public Transmitter { void print_raw() override; /// @brief prints formatted transmitter data void print() override; + /// @brief prints a formatted dashboard of live ET16S values + void print_live_data() override; /// @brief sends mapped data to comms void send_to_comms() override; /// @brief whether the ET16S is in safety mode, determined if switch a is in forward position or if the ET16S is disconnected. diff --git a/src/sensors/transmitter/dr16.cpp b/src/sensors/transmitter/dr16.cpp index 13c615ce4..d34280156 100644 --- a/src/sensors/transmitter/dr16.cpp +++ b/src/sensors/transmitter/dr16.cpp @@ -409,3 +409,21 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM feed = last_feed; } } +void DR16::print_live_data() { + Serial.printf("=== LIVE DR16 TRANSMITTER DATA ===\n"); + Serial.printf(" Safety Mode : %s\n", is_safety_mode() ? "ON" : "OFF"); + Serial.printf(" Control Mode: %s\n", is_teensy_mode() ? "TEENSY" : (is_hive_mode() ? "HIVE" : "UNKNOWN")); + Serial.println("----------------------------------"); + Serial.printf(" L Stick: X: %5.2f | Y: %5.2f\n", get_l_stick_x(), get_l_stick_y()); + Serial.printf(" R Stick: X: %5.2f | Y: %5.2f\n", get_r_stick_x(), get_r_stick_y()); + Serial.printf(" Wheel : %5.2f\n", get_wheel()); + Serial.printf(" L Switch: %d | R Switch: %d\n", (int)get_l_switch(), (int)get_r_switch()); + Serial.println("------------- MOUSE --------------"); + Serial.printf(" X: %5d | Y: %5d | Z: %5d\n", get_mouse_x(), get_mouse_y(), get_mouse_z()); + Serial.printf(" L_Btn: %d | R_Btn: %d\n", get_l_mouse_button(), get_r_mouse_button()); + Serial.println("------------ KEYBOARD ------------"); + + Keys k = get_keys(); + Serial.printf(" W:%d A:%d S:%d D:%d | Q:%d E:%d\n", k.w, k.a, k.s, k.d, k.q, k.e); + Serial.printf(" Shift:%d | Ctrl:%d\n", k.shift, k.ctrl); +} diff --git a/src/sensors/transmitter/dr16.hpp b/src/sensors/transmitter/dr16.hpp index fe4a3ca0d..28520cf28 100644 --- a/src/sensors/transmitter/dr16.hpp +++ b/src/sensors/transmitter/dr16.hpp @@ -143,6 +143,8 @@ class DR16 : public Transmitter { /// @brief Prints the raw 18-byte packet from the receiver void print_raw() override; + /// @brief Prints a formatted dashboard of live DR16 values + void print_live_data() override; /// @brief Get mouse velocity x /// @return Amount of points since last read diff --git a/src/sensors/transmitter/transmitter.hpp b/src/sensors/transmitter/transmitter.hpp index 7f256b2d5..5ae7520fe 100644 --- a/src/sensors/transmitter/transmitter.hpp +++ b/src/sensors/transmitter/transmitter.hpp @@ -16,6 +16,8 @@ class Transmitter { /// @brief prints all output values virtual void print() = 0; + /// @brief Prints a formatted dashboard of live transmitter values + virtual void print_live_data() = 0; /// @brief prints raw packet values for debugging virtual void print_raw() = 0; /// @brief sends data to comms diff --git a/src/sensors/transmitter/transmitter_manager.cpp b/src/sensors/transmitter/transmitter_manager.cpp index e7defcbbb..f5635acde 100644 --- a/src/sensors/transmitter/transmitter_manager.cpp +++ b/src/sensors/transmitter/transmitter_manager.cpp @@ -26,6 +26,14 @@ void TransmitterManager::read() { } } +void TransmitterManager::print_live_data() { + if (transmitter) { + transmitter->print_live_data(); + } else { + safety::safety_procedure("TransmitterManager::print_live_data called before transmitter was initialized"); + } +} + void TransmitterManager::send_to_comms() { if (transmitter) { transmitter->send_to_comms(); diff --git a/src/sensors/transmitter/transmitter_manager.hpp b/src/sensors/transmitter/transmitter_manager.hpp index 1ea5468a2..b2c2c0767 100644 --- a/src/sensors/transmitter/transmitter_manager.hpp +++ b/src/sensors/transmitter/transmitter_manager.hpp @@ -15,6 +15,8 @@ class TransmitterManager { void init(const Cfg::Transmitter& transmitter_config); /// @brief Reads data from the transmitter void read(); + /// @brief Prints the formatted live dashboard of the active transmitter + void print_live_data(); /// @brief Sends data from the transmitter to comms void send_to_comms(); /// @brief Whether the transmitter is currently in safety mode. @@ -38,4 +40,4 @@ class TransmitterManager { std::unique_ptr transmitter; -}; \ No newline at end of file +}; diff --git a/src/utils/profiler.cpp b/src/utils/profiler.cpp index 1ecc827e4..28ca86a13 100644 --- a/src/utils/profiler.cpp +++ b/src/utils/profiler.cpp @@ -11,17 +11,14 @@ void Profiler::clear() { sec = profiler_section_t(); #endif } - void Profiler::begin(const char *name) { #ifdef PROFILER - // find section by name or first empty slot for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) { - if (strcmp(sections[i].name, name) == 0 || (sections[i].count == 0 && sections[i].overflowed == 0)) { - // start section + if (strcmp(sections[i].name, name) == 0 || (sections[i].count == 0 && sections[i].name[0] == '\0')) { strncpy(sections[i].name, name, PROF_MAX_NAME); - sections[i].name[PROF_MAX_NAME] = '\0'; // ensure null termination + sections[i].name[PROF_MAX_NAME] = '\0'; sections[i].start_time = micros(); - sections[i].started = 1; // label section as started + sections[i].started = 1; return; } } @@ -30,88 +27,48 @@ void Profiler::begin(const char *name) { void Profiler::end(const char *name) { #ifdef PROFILER - // find section by name for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) { if (strcmp(sections[i].name, name) == 0) { - // end section - if (sections[i].count < PROF_MAX_TIMES) { - sections[i].time_lengths[sections[i].count] = micros() - sections[i].start_time; - sections[i].count++; - sections[i].started = 0; // label section as finished - - // if count is now at limit, reset count and label as overflowed - if (sections[i].count == PROF_MAX_TIMES) { - sections[i].count = 0; - sections[i].overflowed = 1; - } - } - return; - } - } -#endif -} - -void Profiler::print(const char *name) { -#ifdef PROFILER - uint32_t sum = 0; - uint32_t min = UINT32_MAX; - uint32_t max = 0; + if (!sections[i].started) return; - // find section by name - for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) { - if (strcmp(sections[i].name, name) == 0) { - // calculate values - uint32_t trueCount = sections[i].overflowed ? PROF_MAX_TIMES : sections[i].count; - for (uint32_t j = 0; j < trueCount; j++) { - // if the last run was started and not ended, ignore it - if (sections[i].started && j == sections[i].count) - continue; + uint32_t delta = micros() - sections[i].start_time; - uint32_t delta = sections[i].time_lengths[j]; - sum += delta; - if (delta < min) - min = delta; - if (delta > max) - max = delta; + // Running Average Math + if (sections[i].count == 0) { + // First run: set absolute baselines + sections[i].avg_time = (float)delta; + sections[i].max_time = delta; + } else { + // Subsequent runs: Moving Average + sections[i].avg_time = (sections[i].avg_time * 0.99f) + ((float)delta * 0.01f); + + // Track absolute max + if (delta > sections[i].max_time) { + sections[i].max_time = delta; + } } - // print values - Serial.printf("Profiling for: %s\n Min: %u us\n Max: %u us\n Avg: %u us\n", name, min, max, - (trueCount == 0 ? 0 : sum / trueCount)); + sections[i].count++; + sections[i].started = 0; return; } } #endif } + void Profiler::print_summary() { #ifdef PROFILER Serial.println("\n================ PROFILER SUMMARY ================"); Serial.println(" Subsystem | Avg Time (us) | Max Time (us) "); Serial.println("--------------------------------------------------"); - // Iterate through all possible sections for (uint32_t i = 0; i < PROF_MAX_SECTIONS; i++) { - // If the section actually has a name, it's active - if (sections[i].name[0] != '\0') { - uint32_t sum = 0; - uint32_t max = 0; - - uint32_t trueCount = sections[i].overflowed ? PROF_MAX_TIMES : sections[i].count; - if (trueCount == 0) continue; // Skip if no data yet - - for (uint32_t j = 0; j < trueCount; j++) { - // if the last run was started and not ended, ignore it - if (sections[i].started && j == sections[i].count) continue; - - uint32_t delta = sections[i].time_lengths[j]; - sum += delta; - if (delta > max) max = delta; - } - - uint32_t avg = sum / trueCount; + if (sections[i].name[0] != '\0' && sections[i].count > 0) { - // %-16s pads the string to 16 characters so the table columns align perfectly - Serial.printf(" %-16s | %13u | %13u \n", sections[i].name, avg, max); + Serial.printf(" %-16s | %13u | %13u \n", + sections[i].name, + (uint32_t)sections[i].avg_time, + sections[i].max_time); } } Serial.println("==================================================\n"); diff --git a/src/utils/profiler.hpp b/src/utils/profiler.hpp index 128f0b9e9..e614350f7 100644 --- a/src/utils/profiler.hpp +++ b/src/utils/profiler.hpp @@ -17,14 +17,14 @@ struct Profiler { /// @brief Data structure for a profiling section struct profiler_section_t { - /// @brief the time lengths of each profiling section - uint32_t time_lengths[PROF_MAX_TIMES] = {0}; /// @brief Start time for the current profiling section uint32_t start_time = 0; + /// @brief Average time of section + float avg_time = 0.0f; + /// @brief max time of section + uint32_t max_time = 0; /// @brief Number of start/end times recorded uint16_t count = 0; - /// @brief Label on if count has overflowed - uint8_t overflowed = 0; /// @brief Label on if a begin() has been called and the corresponding end() hasn't yet uint8_t started = 0; /// @brief Name for each section @@ -44,8 +44,8 @@ struct Profiler { /// @brief Print stats for a profiling section /// @param name The name of the section to print stats for - void print(const char *name); - + // void print(const char *name); + /// @brief print formatted summary of all sections void print_summary(); }; From 90de6b5ae0e9f2cfd530af752f435b5d1c390c14 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 23:21:41 -0600 Subject: [PATCH 08/41] CLI now supports multiple views at the same time --- src/hello_robot.cpp | 230 +++++++++++++++++++++++++------------------- 1 file changed, 129 insertions(+), 101 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 0183c7cb7..9dd9b4cfc 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -251,141 +251,169 @@ void HelloRobot::loop_timing(){ // Keep the loop running at the desired rate loop_timer.delay_micros((int)(1E6 / (float)(LOOP_FREQ))); } -void HelloRobot::process_cli(){ - static uint32_t last_redraw_time = 0; - static uint32_t redraw_interval = 1000; // Time in ms between frames - enum class LiveMode { NONE, PROFILER_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE,HEARTBEAT ,SENSORS}; +void HelloRobot::process_cli() { + // ========================================== + // 1. LIVE VIEW DEFINITIONS + // ========================================== + enum class LiveMode { NONE, PROFILE_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE, SENSORS, HEARTBEAT }; - static LiveMode current_live_mode = LiveMode::NONE; - - // ========================================== - // LIVE VIEW + const uint8_t MAX_LIVE_VIEWS = 4; + static LiveMode active_views[MAX_LIVE_VIEWS]; + static uint8_t num_active_views = 0; + + static uint32_t last_redraw_time = 0; + static uint32_t redraw_interval = 1000; + // ========================================== - if (current_live_mode != LiveMode::NONE) { + // 2. LIVE VIEW RENDERER + // ========================================== + if (num_active_views > 0) { - // Check if it's time to draw the next frame if (millis() - last_redraw_time >= redraw_interval) { Serial.print("\033[H"); // Move cursor to top-left - // Draw whichever screen is currently active - switch (current_live_mode) { - case LiveMode::PROFILER_VIEW: - prof.print_summary(); - break; - - case LiveMode::TRANSMITTER: - transmitter_manager.print_live_data(); - break; + // Loop through the array and draw the views in the exact order the user typed them + for (int i = 0; i < num_active_views; i++) { + switch (active_views[i]) { + case LiveMode::PROFILE_VIEW: +#ifdef PROFILER + prof.print_summary(); +#endif + break; + + case LiveMode::TRANSMITTER: + transmitter_manager.print_live_data(); + break; + + case LiveMode::SENSORS: + Serial.printf("=== LIVE SENSOR READOUT ===\n"); + sensor_manager.print_sensors_live(); + break; + + case LiveMode::ESTIMATED_STATE: + Serial.printf("=== LIVE ESTIMATED STATE ===\n"); + estimated_state_map->print(); + break; - case LiveMode::ESTIMATED_STATE: - Serial.printf("=== LIVE ESTIMATED STATE ===\n"); - estimated_state_map->print(); - break; - - case LiveMode::TARGET_STATE: - Serial.printf("=== LIVE TARGET STATE ===\n"); - target_state_map->print(); - break; - - case LiveMode::HEARTBEAT: - Serial.printf("=== LIVE HEARTBEAT ===\n"); - Serial.println(loopc); - break; - case LiveMode::SENSORS: - sensor_manager.print_sensors_live(); - break; - default: - break; + case LiveMode::TARGET_STATE: + Serial.printf("=== LIVE TARGET STATE ===\n"); + target_state_map->print(); + break; + + case LiveMode::HEARTBEAT: + Serial.printf("=== LIVE HEARTBEAT ===\n"); + Serial.println(loopc); + break; + + default: + break; + } + Serial.println(); // Add a blank line between stacked views } Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); + + // Critical: \033[J clears everything *below* the cursor. + // This prevents "ghost text" from getting left behind if a tall view + // updates and suddenly becomes shorter. + Serial.print("\033[J"); + last_redraw_time = millis(); } - // If the user types anything, exit live mode + // Exit live mode on any keystroke if (Serial.available() > 0) { - current_live_mode = LiveMode::NONE; - - // Flush the buffer - while(Serial.available()) Serial.read(); - + num_active_views = 0; // Empty the array + while(Serial.available()) Serial.read(); // Flush buffer Serial.println("\n\n[Exited Live View]"); cli_index = 0; } - return; // Return immediately so normal parsing doesn't run + return; } - // Read all available characters in the hardware buffer + + // ========================================== + // 3. NORMAL CLI PARSING + // ========================================== + /* Adding new commands is simple. + add if statement to cmd elseif block below + If it is live add to live mode enum and switch statement above + as well as to cmd elseif block below + */ while (Serial.available() > 0) { char c = Serial.read(); - // If the character is a newline or carriage return, the command is complete + if (c == '\n' || c == '\r') { - if (cli_index == 0) continue; // Ignore empty lines + if (cli_index == 0) continue; - cli_buffer[cli_index] = '\0'; // Null-terminate the string + cli_buffer[cli_index] = '\0'; String cmd = String(cli_buffer); - cmd.trim(); - - // ========================================== - // COMMAND DICTIONARY - // ========================================== - /* Adding new commands is simple. - add if statement to cmd elseif block below - If it is live add to live mode enum and switch statement above - as well as to cmd elseif block below - */ + cmd.trim(); + if (cmd == "ping") { Serial.println("pong! Robot is alive."); } - else if (cmd == "print tx") { - current_live_mode = LiveMode::TRANSMITTER; - redraw_interval = 100; // 10Hz - last_redraw_time = 0; - Serial.print("\033[2J"); - } - else if (cmd == "print dt") { - Serial.printf("Last loop dt: %f seconds\n", stall_timer.delta()); - } - else if (cmd == "prof") { - current_live_mode = LiveMode::PROFILER_VIEW; - redraw_interval = 1000; // 1Hz - last_redraw_time = 0; // Force immediate redraw - Serial.print("\033[2J"); // Clear screen + // --- THE PARSER --- + else if (cmd.startsWith("live ")) { + num_active_views = 0; + redraw_interval = 1000; // Default to slow refresh + + // Start reading after the word "live " (index 5) + int startIndex = 5; + + // Parse the command word by word + while (startIndex < (int)cmd.length() && num_active_views < MAX_LIVE_VIEWS) { + int spaceIndex = cmd.indexOf(' ', startIndex); + if (spaceIndex == -1) spaceIndex = cmd.length(); // End of string + + // Extract the word + String arg = cmd.substring(startIndex, spaceIndex); + arg.trim(); + + // Check the word and push the corresponding view to the stack + if (arg == "prof") { + active_views[num_active_views++] = LiveMode::PROFILE_VIEW; + } + else if (arg == "tx") { + active_views[num_active_views++] = LiveMode::TRANSMITTER; + redraw_interval = 100; // If TX is anywhere in the stack, speed up the refresh rate + } + else if (arg == "sensors") { + active_views[num_active_views++] = LiveMode::SENSORS; + redraw_interval = 100; // If Sensors are active, speed up the refresh rate + } + else if (arg == "target_state") { + active_views[num_active_views++] = LiveMode::TARGET_STATE; + redraw_interval = 100; // If Sensors are active, speed up the refresh rate + } + else if (arg == "estimated_state") { + active_views[num_active_views++] = LiveMode::ESTIMATED_STATE; + redraw_interval = 100; // If Sensors are active, speed up the refresh rate + } + else if (arg == "heartbeat") { + active_views[num_active_views++] = LiveMode::HEARTBEAT; + redraw_interval = 100; // If Sensors are active, speed up the refresh rate + } + + // Move to the next word + startIndex = spaceIndex + 1; + } + + if (num_active_views > 0) { + last_redraw_time = 0; // Force immediate redraw + Serial.print("\033[2J"); // Clear screen to prep the canvas + } else { + Serial.println("Usage: live [prof] [tx] [sensors]"); + } } - else if (cmd == "print estimated state"){ - current_live_mode = LiveMode::ESTIMATED_STATE; - redraw_interval = 500; // 2Hz update - last_redraw_time = 0; - Serial.print("\033[2J"); - } - else if (cmd == "print target state"){ - current_live_mode = LiveMode::TARGET_STATE; - redraw_interval = 500; // 2Hz update - last_redraw_time = 0; - Serial.print("\033[2J"); - } - else if (cmd == "print sensors") { - current_live_mode = LiveMode::SENSORS; - redraw_interval = 100; - last_redraw_time = 0; - Serial.print("\033[2J"); - } - else if (cmd== "heartbeat"){ - current_live_mode = LiveMode::HEARTBEAT; - redraw_interval = 500; // 2Hz update - last_redraw_time = 0; - Serial.print("\033[2J"); - } else { - Serial.println("Unknown command. Try: ping, print dt, prof, print estimated state, heartbeat"); - } + Serial.println("Unknown command. Try: ping, live prof tx"); + } - // Reset the buffer for the next command cli_index = 0; } else if (cli_index < 63) { - // Add the typed character to the buffer cli_buffer[cli_index++] = c; } } From 08df1f15e1dee0f58a9aba2882ba8bdee890a2dd Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 23:52:46 -0600 Subject: [PATCH 09/41] we have logger now too lol --- src/hello_robot.cpp | 4 ++- src/hello_robot.hpp | 8 +++--- src/utils/system_log.cpp | 61 ++++++++++++++++++++++++++++++++++++++++ src/utils/system_log.hpp | 33 ++++++++++++++++++++++ 4 files changed, 101 insertions(+), 5 deletions(-) create mode 100644 src/utils/system_log.cpp create mode 100644 src/utils/system_log.hpp diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 9dd9b4cfc..270fb9450 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -311,7 +311,7 @@ void HelloRobot::process_cli() { } Serial.println(); // Add a blank line between stacked views } - + SystemLog.draw_dashboard_box(); // puts all non-CLI prints in neat box Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); // Critical: \033[J clears everything *below* the cursor. @@ -325,6 +325,7 @@ void HelloRobot::process_cli() { // Exit live mode on any keystroke if (Serial.available() > 0) { num_active_views = 0; // Empty the array + SystemLog.is_live_view_active = false; //Turn standard scrolling prints back on while(Serial.available()) Serial.read(); // Flush buffer Serial.println("\n\n[Exited Live View]"); cli_index = 0; @@ -357,6 +358,7 @@ void HelloRobot::process_cli() { // --- THE PARSER --- else if (cmd.startsWith("live ")) { num_active_views = 0; + SystemLog.is_live_view_active = true; redraw_interval = 1000; // Default to slow refresh // Start reading after the word "live " (index 5) diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index e477ca749..84d84d08f 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -23,11 +23,11 @@ #include "controls/estimator_manager.hpp" #include "sensors/StereoCamTrigger.hpp" #include "sensors/RefSystem.hpp" -#include "utils/profiler.hpp" - #include "sensor_manager.hpp" -#include +#include +#include "utils/profiler.hpp" +#include "utils/system_log.hpp" #include "comms/data/hive_data.hpp" #include "comms/data/sendable.hpp" @@ -45,7 +45,7 @@ class HelloRobot { Profiler prof; #endif CANManager can; - //RefSystem ref; + TransmitterManager transmitter_manager; //Comms::CommsLayer comms_layer; diff --git a/src/utils/system_log.cpp b/src/utils/system_log.cpp new file mode 100644 index 000000000..d5e2de2db --- /dev/null +++ b/src/utils/system_log.cpp @@ -0,0 +1,61 @@ +#include "system_log.hpp" + +// Instantiate the global logger +SystemLogger SystemLog; + +size_t SystemLogger::write(uint8_t c) { + // Ignore carriage returns to prevent weird spacing + if (c == '\r') return 1; + + // If we receive a newline (e.g. from println), or if the buffer is full, + // finalize the message and push it to the circular array. + if (c == '\n' || line_length >= MAX_LINE_LEN - 1) { + push_message(); + } else { + current_line[line_length++] = (char)c; + } + + return 1; // Tell the Print class we successfully wrote 1 byte +} + +size_t SystemLogger::write(const uint8_t *buffer, size_t size) { + // This override makes bulk printing (like Strings) much faster + for (size_t i = 0; i < size; i++) { + write(buffer[i]); + } + return size; +} + +void SystemLogger::push_message() { + current_line[line_length] = '\0'; // Null-terminate the string + + // Prepend the timestamp and format it into the actual circular buffer + snprintf(messages[head], MAX_STORED_LEN, "[%7.2fs] %s", millis() / 1000.0f, current_line); + + // If the dashboard is CLOSED, print immediately to the scrolling terminal + if (!is_live_view_active) { + Serial.println(messages[head]); + } + + // Advance the circular buffer indices + head = (head + 1) % LOG_HISTORY; + if (count < LOG_HISTORY) count++; + + // Reset the temporary line buffer for the next message + line_length = 0; +} + +void SystemLogger::draw_dashboard_box() { + Serial.println("============= SYSTEM EVENT LOG ============="); + if (count == 0) { + Serial.println(" No recent events."); + } else { + // Print from oldest to newest + uint8_t start = (count == LOG_HISTORY) ? head : 0; + for (uint8_t i = 0; i < count; i++) { + uint8_t idx = (start + i) % LOG_HISTORY; + Serial.printf(" %s\n", messages[idx]); + } + } + Serial.println("============================================"); +} diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp new file mode 100644 index 000000000..6ba75416a --- /dev/null +++ b/src/utils/system_log.hpp @@ -0,0 +1,33 @@ +#pragma once +#include + +class SystemLogger : public Print { +private: + static const int LOG_HISTORY = 6; + static const int MAX_LINE_LEN = 80; // Max length of the user's print statement + static const int MAX_STORED_LEN = 100; // Max length including the timestamp + char messages[LOG_HISTORY][MAX_LINE_LEN] = {0}; + uint8_t head = 0; + uint8_t count = 0; + + // A temporary buffer to hold the line while print() is building it + char current_line[MAX_LINE_LEN] = {0}; + uint8_t line_length = 0; + + // Helper to finalize the line and push it to history + void push_message(); + +public: + bool is_live_view_active = false; + + // This is the ONLY function we have to implement to satisfy the Print class + size_t write(uint8_t c) override; + + // Optional but speeds up printf() and String printing significantly + size_t write(const uint8_t *buffer, size_t size) override; + + void draw_dashboard_box(); +}; + +// Declare a global instance so you can use it everywhere, just like 'Serial' +extern SystemLogger SystemLog; From f5e7e5eb7f47611581c04296a662005e036d0c10 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 18 Apr 2026 23:57:53 -0600 Subject: [PATCH 10/41] changed out some serial prints to systemlogger in main --- src/hello_robot.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 270fb9450..39ed715cb 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -140,7 +140,7 @@ void HelloRobot::process_behaviors(){ // clear the request Comms::comms_layer.get_hive_data().override_state_data.active = false; - Serial.printf("Overriding state with hive state\n"); + SystemLog.printf("Overriding state with hive state\n"); hive_state_map_offset->from_comms_packet(Comms::comms_layer.get_hive_data().override_state_data.state); *estimated_state_map = *hive_state_map_offset; @@ -156,7 +156,7 @@ void HelloRobot::update_controls(){ if ((feed - (*estimated_state_map)[Cfg::StateName::Feeder].get_position() > 2 && transmitter_manager.is_teensy_mode()) || ((*target_state_map)[Cfg::StateName::Feeder].get_position() - (*estimated_state_map)[Cfg::StateName::Feeder].get_position() > 2 && transmitter_manager.is_hive_mode())) { - Serial.printf("Feeder is lowkey jammed. current ball count: %f, feed: %f, hive target: %f\n", + SystemLog.printf("Feeder is lowkey jammed. current ball count: %f, feed: %f, hive target: %f\n", (*estimated_state_map)[Cfg::StateName::Feeder].get_position(), feed, (*target_state_map)[Cfg::StateName::Feeder].get_position()); feed = (*estimated_state_map)[Cfg::StateName::Feeder].get_position() + 1; governor->set_position_reference(Cfg::StateName::Feeder, feed); @@ -191,13 +191,13 @@ void HelloRobot::check_safety(){ // zero the can bus just in case can.issue_safety_mode(); - Serial.printf("Slow loop with dt: %f, slow loop count %d\n", dt, slow_loop_counter); + SystemLog.printf("Slow loop with dt: %f, slow loop count %d\n", dt, slow_loop_counter); // mark this as a slow loop to trigger safety mode is_slow_loop = true; if (last_loop_slow) { slow_loop_counter++; if (slow_loop_counter > 10) { - Serial.printf("Kowabunga bitches\n"); + SystemLog.printf("Kowabunga bitches\n"); reset_teensy(); } } else { From f74df6e00e5392eb77316da150a2b678280d03b4 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 18:34:02 -0600 Subject: [PATCH 11/41] added release and dev flags for make --- Makefile | 10 +++++++++- src/utils/profiler.hpp | 3 ++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index fe75c9f84..15f4d56c2 100644 --- a/Makefile +++ b/Makefile @@ -50,7 +50,14 @@ INCLUDE_FLAGS := $(TEENSY_INC_FLAGS) $(LIBRARY_INC_FLAGS) $(SRC_INC_FLAGS) # Compiler flags specific to Teensy 4.1 TEENSY4_FLAGS = -DF_CPU=600000000 -DUSB_CUSTOM -DLAYOUT_US_ENGLISH -D__IMXRT1062__ -DTEENSYDUINO=159 -DARDUINO_TEENSY41 -DARDUINO=10813 -DFIRMWARE +ifneq ($(filter debug,$(MAKECMDGOALS)),) + # If "make debug" is run, append the profiler macro + TEENSY_FLAGS += -DPROFILER +endif +ifneq ($(filter release,$(MAKECMDGOALS)),) + # (Optional) If "make release" is run, you can add explicit release flags here later +endif # CPU flags to optimize code for the Teensy processor CPU_CFLAGS = -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-d16 -mthumb @@ -116,7 +123,8 @@ MAKEFLAGS += -j$(nproc) # Phony target to force a build every time .PHONY: build - +debug: clangd $(BUILD_DIR)/$(TARGET_EXEC) +release: clangd $(BUILD_DIR)/$(TARGET_EXEC) # Main build target; depends on the target executable and git scraper build: clangd $(BUILD_DIR)/$(TARGET_EXEC) diff --git a/src/utils/profiler.hpp b/src/utils/profiler.hpp index e614350f7..ed907dac3 100644 --- a/src/utils/profiler.hpp +++ b/src/utils/profiler.hpp @@ -2,7 +2,8 @@ #define PROFILER_H // Use this flag to toggle profiling globally. -#define PROFILER +//#define PROFILER +//JUST USE make debug #include From bb08fa806e11b5bc4c2c3aa5423642535d1b71f5 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 19:19:39 -0600 Subject: [PATCH 12/41] added help statement and logger comments --- src/hello_robot.cpp | 34 ++++++++++++++++++++++++++++++++-- src/utils/system_log.hpp | 8 ++++---- 2 files changed, 36 insertions(+), 6 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 39ed715cb..c1dfe3199 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -354,8 +354,38 @@ void HelloRobot::process_cli() { if (cmd == "ping") { Serial.println("pong! Robot is alive."); - } - // --- THE PARSER --- + } + else if (cmd == "help") { + Serial.println("NAME"); + Serial.println(" Robot CLI - Control and monitor firmware"); + Serial.println(); + Serial.println("SYNOPSIS"); + Serial.println(" [command] [arguments...]"); + Serial.println(); + Serial.println("DESCRIPTION"); + Serial.println(" Provides a serial interface to interact with the robot, check"); + Serial.println(" connection status, and launch live, real-time data dashboards."); + Serial.println(); + Serial.println("COMMANDS"); + Serial.println(" ping"); + Serial.println(" Replies with 'pong!' to verify the serial connection is active."); + Serial.println(); + Serial.println(" live [view1] [view2] ..."); + Serial.println(" Launches a live updating dashboard with the specified views."); + Serial.println(" Views are stacked vertically in the order provided."); + Serial.println(" Press ANY KEY to exit live mode."); + Serial.println(); + Serial.println(" Available views:"); + Serial.println(" prof : Execution time profiler (only available if running make debug) "); + Serial.println(" tx : Real-time radio transmitter inputs"); + Serial.println(" sensors : Real-time readouts from all configured sensors"); + Serial.println(" estimated_state : The robot's current estimated state map"); + Serial.println(" target_state : The robot's current target state map"); + Serial.println(" heartbeat : The main loop counter (loopc)"); + Serial.println(); + Serial.println(" help"); + Serial.println(" Displays this manual."); + } // --- THE PARSER --- else if (cmd.startsWith("live ")) { num_active_views = 0; SystemLog.is_live_view_active = true; diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 6ba75416a..fd3ed0e42 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -14,18 +14,18 @@ class SystemLogger : public Print { char current_line[MAX_LINE_LEN] = {0}; uint8_t line_length = 0; - // Helper to finalize the line and push it to history + /// @brief handles whether we print to dashboard or direct to serial void push_message(); public: bool is_live_view_active = false; - // This is the ONLY function we have to implement to satisfy the Print class + /// @brief implements print class print for general print statements. size_t write(uint8_t c) override; - // Optional but speeds up printf() and String printing significantly + /// @brief implements print class print for println,printf,etc... size_t write(const uint8_t *buffer, size_t size) override; - + /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); }; From f7356df9b8e5ea138c0a1b42dd5a17db6a4906bb Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 19:40:27 -0600 Subject: [PATCH 13/41] comments --- src/hello_robot.hpp | 7 +++++-- src/utils/system_log.hpp | 17 ++++++++++++++--- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 23c202e5a..97c9cac48 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -141,9 +141,11 @@ class HelloRobot { /// @brief Hive offset state std::optional hive_state_map_offset; - // CLI Buffer variables + /// @brief CLI Buffer char cli_buffer[64] = {0}; + /// @brief index for cli_buffer uint8_t cli_index = 0; + /// @brief flag for live CLI printing bool live_profiler_active = false; @@ -164,7 +166,8 @@ class HelloRobot { /// @brief LED hearbeat, feeds the watchdog, and ensures consistent loop time. void loop_timing(); - + + /// @brief Command line interface for live printing void process_cli(); public: diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index fd3ed0e42..9a551e589 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -3,27 +3,38 @@ class SystemLogger : public Print { private: + /// @brief number of messages in dashboard box static const int LOG_HISTORY = 6; - static const int MAX_LINE_LEN = 80; // Max length of the user's print statement - static const int MAX_STORED_LEN = 100; // Max length including the timestamp + /// @brief max length of message in dashbarod box + static const int MAX_LINE_LEN = 80; + /// @brief max length including the timestamp + static const int MAX_STORED_LEN = 100; + /// @brief message buffer char messages[LOG_HISTORY][MAX_LINE_LEN] = {0}; + /// @brief start of the message uint8_t head = 0; + /// @brief number of characters in current message uint8_t count = 0; - // A temporary buffer to hold the line while print() is building it + /// @brief A temporary buffer to hold the line while print() is building it char current_line[MAX_LINE_LEN] = {0}; + /// @breif length of print line in buffer uint8_t line_length = 0; /// @brief handles whether we print to dashboard or direct to serial void push_message(); public: + /// @brief flag for live printing from CLI bool is_live_view_active = false; /// @brief implements print class print for general print statements. + /// @brief c is message to be written size_t write(uint8_t c) override; /// @brief implements print class print for println,printf,etc... + /// @params buffer with message + /// @params size of message size_t write(const uint8_t *buffer, size_t size) override; /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); From 6c76bb145433750bf170b78e04fee4d565f5ce1d Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 19:42:30 -0600 Subject: [PATCH 14/41] more comments --- src/utils/system_log.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 9a551e589..d566fdb7c 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -18,7 +18,7 @@ class SystemLogger : public Print { /// @brief A temporary buffer to hold the line while print() is building it char current_line[MAX_LINE_LEN] = {0}; - /// @breif length of print line in buffer + /// @brief length of print line in buffer uint8_t line_length = 0; /// @brief handles whether we print to dashboard or direct to serial @@ -33,8 +33,8 @@ class SystemLogger : public Print { size_t write(uint8_t c) override; /// @brief implements print class print for println,printf,etc... - /// @params buffer with message - /// @params size of message + /// @param *buffer with message + /// @param size of message size_t write(const uint8_t *buffer, size_t size) override; /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); From 88fc0078c3224b4977dfa8c63a90534362e7a69b Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 19:43:32 -0600 Subject: [PATCH 15/41] maybe last comment --- src/utils/system_log.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index d566fdb7c..b61a8b62c 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -1,6 +1,6 @@ #pragma once #include - +/// @brief Serial wrapper for handling print statements class SystemLogger : public Print { private: /// @brief number of messages in dashboard box From 9cf2a870cd1172c349bc25de595db28a7ff9a8c1 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 19:52:49 -0600 Subject: [PATCH 16/41] added local current_feed variable --- src/hello_robot.cpp | 2 +- src/utils/system_log.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 0ea2ceff1..190362ede 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -151,7 +151,7 @@ void HelloRobot::update_controls(){ estimator_manager.step(*estimated_state_map, override_request); // estimated_state_map.print(); override_request = false; - + float current_feed = (*estimated_state_map)[Cfg::StateName::Feeder].get_position(); if ((feed - (*estimated_state_map)[Cfg::StateName::Feeder].get_position() > 2 && transmitter_manager.is_teensy_mode()) || ((*target_state_map)[Cfg::StateName::Feeder].get_position() - (*estimated_state_map)[Cfg::StateName::Feeder].get_position() > 2 && transmitter_manager.is_hive_mode())) { SystemLog.printf("Feeder is lowkey jammed. current ball count: %f, feed: %f, hive target: %f\n", diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index b61a8b62c..39cb4c01f 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -33,7 +33,7 @@ class SystemLogger : public Print { size_t write(uint8_t c) override; /// @brief implements print class print for println,printf,etc... - /// @param *buffer with message + /// @param buffer with message /// @param size of message size_t write(const uint8_t *buffer, size_t size) override; /// @brief draws dashboard for live prints from CLI From be463b2d047f224e7a202663a8f9b7330b3bd458 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 21:52:30 -0600 Subject: [PATCH 17/41] Makefile now creates unique dir for release and debug --- Makefile | 18 ++++++++++-------- src/hello_robot.cpp | 3 +++ src/hello_robot.hpp | 2 +- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/Makefile b/Makefile index 15f4d56c2..debdbb745 100644 --- a/Makefile +++ b/Makefile @@ -10,6 +10,15 @@ TARGET_EXEC := firmware # Directory where build outputs will be placed BUILD_DIR := ./build +ifneq ($(filter debug,$(MAKECMDGOALS)),) + BUILD_DIR := ./build/debug + PROFILER_FLAG := -DPROFILER +endif + +ifneq ($(filter release,$(MAKECMDGOALS)),) + BUILD_DIR := ./build/release +endif + # Tools directory TOOLS_DIR := ./tools @@ -50,18 +59,11 @@ INCLUDE_FLAGS := $(TEENSY_INC_FLAGS) $(LIBRARY_INC_FLAGS) $(SRC_INC_FLAGS) # Compiler flags specific to Teensy 4.1 TEENSY4_FLAGS = -DF_CPU=600000000 -DUSB_CUSTOM -DLAYOUT_US_ENGLISH -D__IMXRT1062__ -DTEENSYDUINO=159 -DARDUINO_TEENSY41 -DARDUINO=10813 -DFIRMWARE -ifneq ($(filter debug,$(MAKECMDGOALS)),) - # If "make debug" is run, append the profiler macro - TEENSY_FLAGS += -DPROFILER -endif -ifneq ($(filter release,$(MAKECMDGOALS)),) - # (Optional) If "make release" is run, you can add explicit release flags here later -endif # CPU flags to optimize code for the Teensy processor CPU_CFLAGS = -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-d16 -mthumb -DEFINES := $(TEENSY4_FLAGS) +DEFINES := $(TEENSY4_FLAGS) $(PROFILER_FLAG) # Preprocessor flags for both C and C++ files # -MMD: Generate dependency files for each source file diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 190362ede..5b92f901d 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -1,4 +1,7 @@ #include "hello_robot.hpp" +#ifdef PROFILER +Profiler prof; +#endif void HelloRobot::init(){ crash_report(); diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 97c9cac48..293682a30 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -42,7 +42,7 @@ extern "C" void reset_teensy(void); #define HEARTBEAT_FREQ 2 #ifdef PROFILER -Profiler prof; +extern Profiler prof; #endif From faa981e239372a050e52ff2248555c0af6d52257 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 21 Apr 2026 21:57:06 -0600 Subject: [PATCH 18/41] final doxygen comments... --- src/utils/system_log.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 39cb4c01f..2e768c78e 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -29,12 +29,14 @@ class SystemLogger : public Print { bool is_live_view_active = false; /// @brief implements print class print for general print statements. - /// @brief c is message to be written + /// @param c is message to be written + /// @return the message size_t write(uint8_t c) override; /// @brief implements print class print for println,printf,etc... /// @param buffer with message /// @param size of message + /// @return the message size_t write(const uint8_t *buffer, size_t size) override; /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); From c5b55c9282706c4e2c54d2af1be5bd1bd5c20eec Mon Sep 17 00:00:00 2001 From: EricV Date: Wed, 22 Apr 2026 22:12:54 -0600 Subject: [PATCH 19/41] added sliders to et16 and cleaned up print statements --- src/hello_robot.cpp | 8 +++--- src/sensors/AdafruitIMUSensor.cpp | 2 +- src/sensors/transmitter/ET16S.cpp | 44 ++++++++++++++++++++++++------- src/sensors/transmitter/dr16.cpp | 29 +++++++++++++++++--- 4 files changed, 64 insertions(+), 19 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 5b92f901d..385c3a5bf 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -315,11 +315,9 @@ void HelloRobot::process_cli() { Serial.println(); // Add a blank line between stacked views } SystemLog.draw_dashboard_box(); // puts all non-CLI prints in neat box - Serial.println("\n[ LIVE MODE ACTIVE - PRESS ANY KEY TO EXIT ]"); + Serial.println("\n[ LIVE MODE ACTIVE - PRESS ENTER TO EXIT ]"); - // Critical: \033[J clears everything *below* the cursor. - // This prevents "ghost text" from getting left behind if a tall view - // updates and suddenly becomes shorter. + // \033[J clears everything *below* the cursor. Serial.print("\033[J"); last_redraw_time = millis(); @@ -376,7 +374,7 @@ void HelloRobot::process_cli() { Serial.println(" live [view1] [view2] ..."); Serial.println(" Launches a live updating dashboard with the specified views."); Serial.println(" Views are stacked vertically in the order provided."); - Serial.println(" Press ANY KEY to exit live mode."); + Serial.println(" Press ENTER to exit live mode."); Serial.println(); Serial.println(" Available views:"); Serial.println(" prof : Execution time profiler (only available if running make debug) "); diff --git a/src/sensors/AdafruitIMUSensor.cpp b/src/sensors/AdafruitIMUSensor.cpp index 250c58f14..3a0dcd6b6 100644 --- a/src/sensors/AdafruitIMUSensor.cpp +++ b/src/sensors/AdafruitIMUSensor.cpp @@ -25,7 +25,7 @@ void AdafruitIMUSensor::print() { Serial.println(); } void AdafruitIMUSensor::print_live_data() { - Serial.printf("=== LIVE ADAFRUIT IMU DATA ===\n"); + Serial.printf("=== LIVE ADAFRUIT SENSOR DATA ===\n"); // Print temperature Serial.printf(" Temperature: %5.2f °C\n", get_temperature()); diff --git a/src/sensors/transmitter/ET16S.cpp b/src/sensors/transmitter/ET16S.cpp index ad2c7a55c..7bc87fa5d 100644 --- a/src/sensors/transmitter/ET16S.cpp +++ b/src/sensors/transmitter/ET16S.cpp @@ -108,17 +108,41 @@ void ET16S::print_raw_bin(uint8_t m_inputRaw[ET16S_PACKET_SIZE]) { } void ET16S::print_live_data() { Serial.printf("=== LIVE ET16S TRANSMITTER DATA ===\n"); - Serial.printf(" Safety Mode: %s\n", is_safety_mode() ? "ON" : "OFF"); - Serial.printf(" Control Mode: %s\n", is_teensy_mode() ? "TEENSY" : (is_hive_mode() ? "HIVE" : "UNKNOWN")); - Serial.println("-----------------------------------"); - Serial.printf(" L Stick: X: %5.2f | Y: %5.2f\n", get_l_stick_x(), get_l_stick_y()); - Serial.printf(" R Stick: X: %5.2f | Y: %5.2f\n", get_r_stick_x(), get_r_stick_y()); - Serial.printf(" L Dial : %5.2f | R Dial : %5.2f\n", get_l_dial(), get_r_dial()); - Serial.println("-----------------------------------"); - Serial.printf(" SW_B: %d | SW_C: %d | SW_D: %d | SW_E: %d\n", - (int)get_switch_b(), (int)get_switch_c(), (int)get_switch_d(), (int)get_switch_e()); + + const char* mode_str = "UNKNOWN"; + if (is_safety_mode()) { + mode_str = "SAFETY"; + } else if (is_teensy_mode()) { + mode_str = "TEENSY"; + } else if (is_hive_mode()) { + mode_str = "HIVE"; + } + + Serial.printf(" Control Mode: %-7s\n", mode_str); + + Serial.println("---------------------------------------"); + Serial.printf(" L Stick : X: %5.2f | Y: %5.2f\n", get_l_stick_x(), get_l_stick_y()); + Serial.printf(" R Stick : X: %5.2f | Y: %5.2f\n", get_r_stick_x(), get_r_stick_y()); + Serial.printf(" L Dial : %5.2f | R Dial : %5.2f\n", get_l_dial(), get_r_dial()); + Serial.printf(" L Slider: %5.2f | R Slider: %5.2f\n", get_l_slider(), get_r_slider()); + Serial.println("---------------------------------------"); + + // Lambda to convert the float value into the SwitchPos string + auto sw_str = [](auto val) -> const char* { + switch (static_cast(static_cast(val))) { + case SwitchPos::FORWARD: return "FORWARD"; + case SwitchPos::BACKWARD: return "BACKWARD"; + case SwitchPos::MIDDLE: return "MIDDLE"; + default: return "INVALID"; + } + }; + + // Print using the lambda and the %-8s padding to prevent text ghosting + Serial.printf(" SW_B: %-8s | SW_C: %-8s\n", sw_str(get_switch_b()), sw_str(get_switch_c())); + Serial.printf(" SW_D: %-8s | SW_E: %-8s\n", sw_str(get_switch_d()), sw_str(get_switch_e())); + Serial.printf(" SW_F: %-8s | SW_G: %-8s\n", sw_str(get_switch_f()), sw_str(get_switch_g())); + Serial.printf(" SW_H: %-8s |\n", sw_str(get_switch_h())); } - void ET16S::print_format_bin(int channel_num) { if (channel_num > ET16S_INPUT_VALUE_COUNT || channel_num < 0) { Serial.print("Invalid channel used for print_format_bin. Must be 0-16"); diff --git a/src/sensors/transmitter/dr16.cpp b/src/sensors/transmitter/dr16.cpp index d34280156..8f1f606d9 100644 --- a/src/sensors/transmitter/dr16.cpp +++ b/src/sensors/transmitter/dr16.cpp @@ -411,13 +411,36 @@ void DR16::manual_controls(const RobotStateMap& estimated_state_map, RobotStateM } void DR16::print_live_data() { Serial.printf("=== LIVE DR16 TRANSMITTER DATA ===\n"); - Serial.printf(" Safety Mode : %s\n", is_safety_mode() ? "ON" : "OFF"); - Serial.printf(" Control Mode: %s\n", is_teensy_mode() ? "TEENSY" : (is_hive_mode() ? "HIVE" : "UNKNOWN")); + + const char* mode_str = "UNKNOWN"; + if (is_safety_mode()) { + mode_str = "SAFETY"; + } else if (is_teensy_mode()) { + mode_str = "TEENSY"; + } else if (is_hive_mode()) { + mode_str = "HIVE"; + } + + Serial.printf(" Control Mode: %-7s\n", mode_str); + Serial.println("----------------------------------"); Serial.printf(" L Stick: X: %5.2f | Y: %5.2f\n", get_l_stick_x(), get_l_stick_y()); Serial.printf(" R Stick: X: %5.2f | Y: %5.2f\n", get_r_stick_x(), get_r_stick_y()); Serial.printf(" Wheel : %5.2f\n", get_wheel()); - Serial.printf(" L Switch: %d | R Switch: %d\n", (int)get_l_switch(), (int)get_r_switch()); + + // Lambda to convert the float value into the SwitchPos string + auto sw_str = [](auto val) -> const char* { + switch (static_cast(static_cast(val))) { + case SwitchPos::FORWARD: return "FORWARD"; + case SwitchPos::BACKWARD: return "BACKWARD"; + case SwitchPos::MIDDLE: return "MIDDLE"; + default: return "INVALID"; + } + }; + + // Print using the lambda and the %-8s padding + Serial.printf(" L Switch: %-8s | R Switch: %-8s\n", sw_str(get_l_switch()), sw_str(get_r_switch())); + Serial.println("------------- MOUSE --------------"); Serial.printf(" X: %5d | Y: %5d | Z: %5d\n", get_mouse_x(), get_mouse_y(), get_mouse_z()); Serial.printf(" L_Btn: %d | R_Btn: %d\n", get_l_mouse_button(), get_r_mouse_button()); From 1bb0b147f67c91178a400f3d5b837ecd9d234b3b Mon Sep 17 00:00:00 2001 From: EricV Date: Thu, 30 Apr 2026 15:33:58 -0600 Subject: [PATCH 20/41] Changed substr to c style string parsing for better memory management --- src/hello_robot.cpp | 54 ++++++++++++++++++--------------------------- 1 file changed, 21 insertions(+), 33 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 385c3a5bf..485686b7a 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -344,19 +344,18 @@ void HelloRobot::process_cli() { as well as to cmd elseif block below */ while (Serial.available() > 0) { - char c = Serial.read(); - + char c = Serial.read(); + + cli_buffer[cli_index] = '\0'; if (c == '\n' || c == '\r') { if (cli_index == 0) continue; cli_buffer[cli_index] = '\0'; - String cmd = String(cli_buffer); - cmd.trim(); - if (cmd == "ping") { + if (strncmp(cli_buffer, "ping", 4) == 0) { Serial.println("pong! Robot is alive."); } - else if (cmd == "help") { + if (strncmp(cli_buffer, "help", 4) == 0) { Serial.println("NAME"); Serial.println(" Robot CLI - Control and monitor firmware"); Serial.println(); @@ -387,50 +386,39 @@ void HelloRobot::process_cli() { Serial.println(" help"); Serial.println(" Displays this manual."); } // --- THE PARSER --- - else if (cmd.startsWith("live ")) { + else if (strncmp(cli_buffer, "live ", 5) == 0) { num_active_views = 0; SystemLog.is_live_view_active = true; redraw_interval = 1000; // Default to slow refresh - // Start reading after the word "live " (index 5) - int startIndex = 5; - + // The first token will be "live", which we ignore + char* token = strtok(cli_buffer, " "); // Parse the command word by word - while (startIndex < (int)cmd.length() && num_active_views < MAX_LIVE_VIEWS) { - int spaceIndex = cmd.indexOf(' ', startIndex); - if (spaceIndex == -1) spaceIndex = cmd.length(); // End of string - - // Extract the word - String arg = cmd.substring(startIndex, spaceIndex); - arg.trim(); + while ((token = strtok(NULL, " ")) != NULL && num_active_views < MAX_LIVE_VIEWS) { // Check the word and push the corresponding view to the stack - if (arg == "prof") { + if (strcmp(token, "prof") == 0) { active_views[num_active_views++] = LiveMode::PROFILE_VIEW; } - else if (arg == "tx") { + else if (strcmp(token, "tx") == 0) { active_views[num_active_views++] = LiveMode::TRANSMITTER; - redraw_interval = 100; // If TX is anywhere in the stack, speed up the refresh rate - } - else if (arg == "sensors") { + redraw_interval = 100; + } + else if (strcmp(token, "sensors") == 0) { active_views[num_active_views++] = LiveMode::SENSORS; - redraw_interval = 100; // If Sensors are active, speed up the refresh rate - } - else if (arg == "target_state") { + redraw_interval = 100; } + else if (strcmp(token, "target_state") == 0) { active_views[num_active_views++] = LiveMode::TARGET_STATE; - redraw_interval = 100; // If Sensors are active, speed up the refresh rate + redraw_interval = 100; } - else if (arg == "estimated_state") { + else if (strcmp(token, "estimated_state") == 0) { active_views[num_active_views++] = LiveMode::ESTIMATED_STATE; - redraw_interval = 100; // If Sensors are active, speed up the refresh rate + redraw_interval = 100; } - else if (arg == "heartbeat") { + else if (strcmp(token, "heartbeat") == 0) { active_views[num_active_views++] = LiveMode::HEARTBEAT; - redraw_interval = 100; // If Sensors are active, speed up the refresh rate + redraw_interval = 100; } - - // Move to the next word - startIndex = spaceIndex + 1; } if (num_active_views > 0) { From ff28fcefc212b727bff1351051aaf8ab647d2034 Mon Sep 17 00:00:00 2001 From: EricV Date: Fri, 29 May 2026 21:42:42 -0400 Subject: [PATCH 21/41] added context and warning levels to logger --- src/hello_robot.cpp | 54 ++++++++--------- src/utils/system_log.cpp | 124 +++++++++++++++++++++++++++++++++++---- src/utils/system_log.hpp | 48 ++++++++++++--- 3 files changed, 177 insertions(+), 49 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index de0a734c1..193256512 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -274,40 +274,40 @@ void HelloRobot::process_cli() { // Loop through the array and draw the views in the exact order the user typed them for (int i = 0; i < num_active_views; i++) { - switch (active_views[i]) { - case LiveMode::PROFILE_VIEW: + switch (active_views[i]) { + case LiveMode::PROFILE_VIEW: #ifdef PROFILER - prof.print_summary(); + prof.print_summary(); #endif - break; + break; - case LiveMode::TRANSMITTER: - transmitter_manager.print_live_data(); - break; + case LiveMode::TRANSMITTER: + transmitter_manager.print_live_data(); + break; - case LiveMode::SENSORS: - Serial.printf("=== LIVE SENSOR READOUT ===\n"); - sensor_manager.print_sensors_live(); - break; + case LiveMode::SENSORS: + Serial.printf("=== LIVE SENSOR READOUT ===\n"); + sensor_manager.print_sensors_live(); + break; - case LiveMode::ESTIMATED_STATE: - Serial.printf("=== LIVE ESTIMATED STATE ===\n"); - estimated_state_map->print(); - break; + case LiveMode::ESTIMATED_STATE: + Serial.printf("=== LIVE ESTIMATED STATE ===\n"); + estimated_state_map->print(); + break; - case LiveMode::TARGET_STATE: - Serial.printf("=== LIVE TARGET STATE ===\n"); - target_state_map->print(); - break; - - case LiveMode::HEARTBEAT: - Serial.printf("=== LIVE HEARTBEAT ===\n"); - Serial.println(loopc); - break; + case LiveMode::TARGET_STATE: + Serial.printf("=== LIVE TARGET STATE ===\n"); + target_state_map->print(); + break; + + case LiveMode::HEARTBEAT: + Serial.printf("=== LIVE HEARTBEAT ===\n"); + Serial.println(loopc); + break; - default: - break; - } + default: + break; + } Serial.println(); // Add a blank line between stacked views } SystemLog.draw_dashboard_box(); // puts all non-CLI prints in neat box diff --git a/src/utils/system_log.cpp b/src/utils/system_log.cpp index d5e2de2db..a16a238d1 100644 --- a/src/utils/system_log.cpp +++ b/src/utils/system_log.cpp @@ -3,6 +3,31 @@ // Instantiate the global logger SystemLogger SystemLog; +const char* sys_to_str(Subsystem sys) { + switch(sys) { + case Subsystem::CAN: return "CAN"; + case Subsystem::MOTORS: return "MOT"; + case Subsystem::SENSORS: return "SEN"; + case Subsystem::ESTIMATOR: return "EST"; + case Subsystem::COMMS: return "COM"; + case Subsystem::REF: return "REF"; + default: return "GEN"; + } +} + +const char* level_to_color(LogLevel lvl) { + switch(lvl) { + case LogLevel::ERROR: return "\033[31m"; // Red + case LogLevel::WARN: return "\033[33m"; // Yellow + default: return "\033[0m"; // Reset/Terminal Default + } +} + +void SystemLogger::set_context(LogLevel lvl, Subsystem sys) { + current_level = lvl; + current_sys = sys; +} + size_t SystemLogger::write(uint8_t c) { // Ignore carriage returns to prevent weird spacing if (c == '\r') return 1; @@ -19,42 +44,115 @@ size_t SystemLogger::write(uint8_t c) { } size_t SystemLogger::write(const uint8_t *buffer, size_t size) { - // This override makes bulk printing (like Strings) much faster - for (size_t i = 0; i < size; i++) { - write(buffer[i]); - } + for (size_t i = 0; i < size; i++) { write(buffer[i]); } return size; } void SystemLogger::push_message() { - current_line[line_length] = '\0'; // Null-terminate the string + current_line[line_length] = '\0'; - // Prepend the timestamp and format it into the actual circular buffer - snprintf(messages[head], MAX_STORED_LEN, "[%7.2fs] %s", millis() / 1000.0f, current_line); + // 1. Save metadata into the Struct + messages[head].timestamp = millis() / 1000.0f; + messages[head].level = current_level; + messages[head].sys = current_sys; + strncpy(messages[head].text, current_line, MAX_LINE_LEN); - // If the dashboard is CLOSED, print immediately to the scrolling terminal + // 2. If CLI is closed, print immediately with colors! if (!is_live_view_active) { - Serial.println(messages[head]); + Serial.printf("[%7.2fs] %s[%s] %s\033[0m\n", + messages[head].timestamp, + level_to_color(messages[head].level), + sys_to_str(messages[head].sys), + messages[head].text); } - // Advance the circular buffer indices + // 3. Advance circular buffer head = (head + 1) % LOG_HISTORY; if (count < LOG_HISTORY) count++; - // Reset the temporary line buffer for the next message line_length = 0; + current_level = LogLevel::INFO; // Reset to default + current_sys = Subsystem::GENERAL; // Reset to default } +// This bypasses the write() character loop and instantly builds a struct +void SystemLogger::info(Subsystem sys, const char* format, ...) { + char temp[MAX_LINE_LEN]; + va_list args; + va_start(args, format); + vsnprintf(temp, MAX_LINE_LEN, format, args); + va_end(args); + + // Strip trailing newlines so it formats perfectly + size_t len = strlen(temp); + while(len > 0 && (temp[len-1] == '\n' || temp[len-1] == '\r')) temp[--len] = '\0'; + set_context(LogLevel::INFO, sys); + strncpy(current_line, temp, MAX_LINE_LEN); + line_length = len; + push_message(); +} + +void SystemLogger::warn(Subsystem sys, const char* format, ...) { + char temp[MAX_LINE_LEN]; + va_list args; + va_start(args, format); + vsnprintf(temp, MAX_LINE_LEN, format, args); + va_end(args); + size_t len = strlen(temp); + while(len > 0 && (temp[len-1] == '\n' || temp[len-1] == '\r')) temp[--len] = '\0'; + + set_context(LogLevel::WARN, sys); + strncpy(current_line, temp, MAX_LINE_LEN); + line_length = len; + push_message(); +} + +void SystemLogger::error(Subsystem sys, const char* format, ...) { + char temp[MAX_LINE_LEN]; + va_list args; + va_start(args, format); + vsnprintf(temp, MAX_LINE_LEN, format, args); + va_end(args); + size_t len = strlen(temp); + while(len > 0 && (temp[len-1] == '\n' || temp[len-1] == '\r')) temp[--len] = '\0'; + + set_context(LogLevel::ERROR, sys); + strncpy(current_line, temp, MAX_LINE_LEN); + line_length = len; + push_message(); +} void SystemLogger::draw_dashboard_box() { Serial.println("============= SYSTEM EVENT LOG ============="); if (count == 0) { Serial.println(" No recent events."); } else { - // Print from oldest to newest uint8_t start = (count == LOG_HISTORY) ? head : 0; for (uint8_t i = 0; i < count; i++) { uint8_t idx = (start + i) % LOG_HISTORY; - Serial.printf(" %s\n", messages[idx]); + LogEvent& ev = messages[idx]; + + bool show = false; + + // FILTER LOGIC + // Only evaluate if the message meets the minimum requested priority level + if (ev.level >= view_filter_level) { + // Rule 1: High priority (Warnings & Errors) ALWAYS pierce the subsystem filter + if (ev.level >= LogLevel::WARN) { + show = true; + } + // Rule 2: Standard INFO messages only show if they match the active subsystem + else if (view_filter_sys == Subsystem::ALL || ev.sys == view_filter_sys) { + show = true; + } + } + + if (show) { + Serial.printf(" [%7.2fs] %s[%s] %s\033[0m\n", + ev.timestamp, + level_to_color(ev.level), + sys_to_str(ev.sys), + ev.text); + } } } Serial.println("============================================"); diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 2e768c78e..6afd3cc92 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -1,17 +1,30 @@ #pragma once #include + +enum class LogLevel { INFO, WARN, ERROR }; +/// @brief List of robot subsystems +enum class Subsystem { ALL, GENERAL, CAN, MOTORS, SENSORS, ESTIMATOR, COMMS , REF}; + +struct LogEvent { + float timestamp; + LogLevel level; + Subsystem sys; + char text[80]; // Max length of message +}; + /// @brief Serial wrapper for handling print statements class SystemLogger : public Print { private: - /// @brief number of messages in dashboard box - static const int LOG_HISTORY = 6; + /// @brief number of messages in dashboard box + static const int LOG_HISTORY = 10; /// @brief max length of message in dashbarod box - static const int MAX_LINE_LEN = 80; + static const int MAX_LINE_LEN = 80; /// @brief max length including the timestamp static const int MAX_STORED_LEN = 100; - /// @brief message buffer - char messages[LOG_HISTORY][MAX_LINE_LEN] = {0}; - /// @brief start of the message + /// @brief message circuluar buffer + //char messages[LOG_HISTORY][MAX_LINE_LEN] = {{0}}; + LogEvent messages[LOG_HISTORY]; + /// @brief start of the message uint8_t head = 0; /// @brief number of characters in current message uint8_t count = 0; @@ -20,13 +33,20 @@ class SystemLogger : public Print { char current_line[MAX_LINE_LEN] = {0}; /// @brief length of print line in buffer uint8_t line_length = 0; - + + // Context for standard Print() calls + LogLevel current_level = LogLevel::INFO; + Subsystem current_sys = Subsystem::GENERAL; /// @brief handles whether we print to dashboard or direct to serial void push_message(); public: - /// @brief flag for live printing from CLI + /// @brief flag for live printing from CLI bool is_live_view_active = false; + // Dashboard Filters (Defaults to showing everything) + Subsystem view_filter_sys = Subsystem::ALL; + LogLevel view_filter_level = LogLevel::INFO; + /// @brief implements print class print for general print statements. /// @param c is message to be written @@ -37,7 +57,17 @@ class SystemLogger : public Print { /// @param buffer with message /// @param size of message /// @return the message - size_t write(const uint8_t *buffer, size_t size) override; + size_t write(const uint8_t *buffer, size_t size) override; + + // --- NEW: Context setter for multiline Print() blocks --- + void set_context(LogLevel lvl, Subsystem sys); + + // --- NEW: Fast, formatted logger helpers --- + void info(Subsystem sys, const char* format, ...); + void warn(Subsystem sys, const char* format, ...); + void error(Subsystem sys, const char* format, ...); + + /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); }; From b573849a7d5fe6d9777d0c0a3849b0a28dbf5071 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 16:28:53 -0400 Subject: [PATCH 22/41] changed CLI implementation to utilize command pattern for standard commands and a lookup table for live command --- src/hello_robot.cpp | 164 +++++++++++++++++++++++++------------------- src/hello_robot.hpp | 22 +++++- 2 files changed, 112 insertions(+), 74 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index de0a734c1..df732f54d 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -12,7 +12,7 @@ void HelloRobot::init() { // Configure the robot from comms data, which is filled on Hive. Serial.println("Configuring..."); - +/* Comms::comms_layer.configure(); const Cfg::RobotConfig &config = Comms::comms_layer.get_hive_data().config; @@ -48,7 +48,7 @@ void HelloRobot::init() { reference_map.emplace(config.states); target_state_map.emplace(config.states); // Temp ungoverned state hive_state_map_offset.emplace(config.states); // Hive offset state - +*/ // start the main loop watchdog watchdog.start(); } @@ -60,8 +60,9 @@ void HelloRobot::run() { // start main loop time timer stall_timer.start(); - #ifdef PROFILER - prof.begin("Telemetry"); +#ifdef PROFILER + /* + prof.begin("Telemetry"); read_telemetry(); prof.end("Telemetry"); @@ -76,6 +77,7 @@ void HelloRobot::run() { prof.begin("Safety"); check_safety(); prof.end("Safety"); + */ prof.begin("CLI"); process_cli(); prof.end("CLI"); @@ -237,7 +239,7 @@ void HelloRobot::check_safety() { void HelloRobot::loop_timing() { // print loopc every second to verify it is still alive if (loopc % 1000 == 0) { - Serial.println(loopc); + //Serial.println(loopc); } // LED heartbeat -- linked to loop count to reveal slowdowns and // freezes. @@ -252,18 +254,6 @@ void HelloRobot::loop_timing() { } void HelloRobot::process_cli() { - // ========================================== - // 1. LIVE VIEW DEFINITIONS - // ========================================== - enum class LiveMode { NONE, PROFILE_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE, SENSORS, HEARTBEAT }; - - const uint8_t MAX_LIVE_VIEWS = 4; - static LiveMode active_views[MAX_LIVE_VIEWS]; - static uint8_t num_active_views = 0; - - static uint32_t last_redraw_time = 0; - static uint32_t redraw_interval = 1000; - // ========================================== // 2. LIVE VIEW RENDERER // ========================================== @@ -340,18 +330,57 @@ void HelloRobot::process_cli() { as well as to cmd elseif block below */ while (Serial.available() > 0) { - char c = Serial.read(); - - cli_buffer[cli_index] = '\0'; + char c = Serial.read(); + if (c == '\n' || c == '\r') { if (cli_index == 0) continue; cli_buffer[cli_index] = '\0'; - if (strncmp(cli_buffer, "ping", 4) == 0) { - Serial.println("pong! Robot is alive."); + // --- THE COMMAND DICTIONARY --- + static const struct { + const char* name; + void (HelloRobot::*execute)(); + } commands[] = { + {"ping", &HelloRobot::cmd_ping}, + {"help", &HelloRobot::cmd_help}, + {"live", &HelloRobot::cmd_live} + }; + + // --- THE PARSER --- + // 1. Extract the very first word + char* cmd_str = strtok(cli_buffer, " "); + + if (cmd_str != nullptr) { + bool found = false; + + // 2. Scan the dictionary for a match + for (const auto& cmd : commands) { + if (strcmp(cmd_str, cmd.name) == 0) { + // 3. Execute the matched member function + (this->*(cmd.execute))(); + found = true; + break; + } + } + + if (!found) { + Serial.println("Unknown command. Try: help"); + } } - if (strncmp(cli_buffer, "help", 4) == 0) { + + cli_index = 0; + } + else if (cli_index < 63) { + cli_buffer[cli_index++] = c; + } + } +} +void HelloRobot::cmd_ping() { + Serial.println("pong! Robot is alive."); +} + +void HelloRobot::cmd_help() { Serial.println("NAME"); Serial.println(" Robot CLI - Control and monitor firmware"); Serial.println(); @@ -381,57 +410,50 @@ void HelloRobot::process_cli() { Serial.println(); Serial.println(" help"); Serial.println(" Displays this manual."); - } // --- THE PARSER --- - else if (strncmp(cli_buffer, "live ", 5) == 0) { - num_active_views = 0; - SystemLog.is_live_view_active = true; - redraw_interval = 1000; // Default to slow refresh +} +void HelloRobot::cmd_live() { + num_active_views = 0; + SystemLog.is_live_view_active = true; + redraw_interval = 1000; + + struct LiveViewMap { + const char* name; + LiveMode mode; + uint32_t interval; + }; + + static const LiveViewMap view_dict[] = { + {"prof", LiveMode::PROFILE_VIEW, 1000}, + {"tx", LiveMode::TRANSMITTER, 100}, + {"sensors", LiveMode::SENSORS, 100}, + {"target_state", LiveMode::TARGET_STATE, 100}, + {"estimated_state", LiveMode::ESTIMATED_STATE, 100}, + {"heartbeat", LiveMode::HEARTBEAT, 100} + }; + + // --- THE PARSER --- + char* token; + while ((token = strtok(NULL, " ")) != NULL && num_active_views < MAX_LIVE_VIEWS) { + + // Scan the dictionary for a matching view + for (const auto& view : view_dict) { + if (strcmp(token, view.name) == 0) { + // Add the view to the active stack + active_views[num_active_views++] = view.mode; - // The first token will be "live", which we ignore - char* token = strtok(cli_buffer, " "); - // Parse the command word by word - while ((token = strtok(NULL, " ")) != NULL && num_active_views < MAX_LIVE_VIEWS) { - - // Check the word and push the corresponding view to the stack - if (strcmp(token, "prof") == 0) { - active_views[num_active_views++] = LiveMode::PROFILE_VIEW; - } - else if (strcmp(token, "tx") == 0) { - active_views[num_active_views++] = LiveMode::TRANSMITTER; - redraw_interval = 100; - } - else if (strcmp(token, "sensors") == 0) { - active_views[num_active_views++] = LiveMode::SENSORS; - redraw_interval = 100; } - else if (strcmp(token, "target_state") == 0) { - active_views[num_active_views++] = LiveMode::TARGET_STATE; - redraw_interval = 100; - } - else if (strcmp(token, "estimated_state") == 0) { - active_views[num_active_views++] = LiveMode::ESTIMATED_STATE; - redraw_interval = 100; - } - else if (strcmp(token, "heartbeat") == 0) { - active_views[num_active_views++] = LiveMode::HEARTBEAT; - redraw_interval = 100; - } - } - - if (num_active_views > 0) { - last_redraw_time = 0; // Force immediate redraw - Serial.print("\033[2J"); // Clear screen to prep the canvas - } else { - Serial.println("Usage: live [prof] [tx] [sensors]"); + // If this view requires a faster refresh rate, upgrade the global interval + if (view.interval < redraw_interval) { + redraw_interval = view.interval; } + break; // Found a match, break the inner loop to grab the next word } - else { - Serial.println("Unknown command. Try: ping, live prof tx"); - } - - cli_index = 0; - } - else if (cli_index < 63) { - cli_buffer[cli_index++] = c; } } + + if (num_active_views > 0) { + last_redraw_time = 0; + Serial.print("\033[2J"); + } else { + Serial.println("Usage: live [prof] [tx] [sensors] [estimated_state] [target_state] [heartbeat]"); + } } diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index f2d1b9868..d04838fd1 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -137,14 +137,27 @@ class HelloRobot { /// @brief Hive offset state std::optional hive_state_map_offset; - + // ========================================== + // CLI Variables + // ========================================== + enum class LiveMode { NONE, PROFILE_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE, SENSORS, HEARTBEAT }; + + static const uint8_t MAX_LIVE_VIEWS = 4; + LiveMode active_views[MAX_LIVE_VIEWS]; + uint8_t num_active_views = 0; + + uint32_t last_redraw_time = 0; + uint32_t redraw_interval = 1000; /// @brief CLI Buffer char cli_buffer[64] = {0}; /// @brief index for cli_buffer uint8_t cli_index = 0; /// @brief flag for live CLI printing - bool live_profiler_active = false; - + bool live_profiler_active = false; + + // ========================================== + // Major Loop functions + // ========================================== /// @brief check to see if there is a crash report, and if so, print it repeatedly void crash_report(); @@ -165,6 +178,9 @@ class HelloRobot { /// @brief Command line interface for live printing void process_cli(); + void cmd_ping(); + void cmd_help(); + void cmd_live(); public: /** From f2e9a854251ba5050b6600bcf41692c7350221d3 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 16:42:26 -0400 Subject: [PATCH 23/41] removed debug code --- src/hello_robot.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index df732f54d..4798cff20 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -12,7 +12,7 @@ void HelloRobot::init() { // Configure the robot from comms data, which is filled on Hive. Serial.println("Configuring..."); -/* + Comms::comms_layer.configure(); const Cfg::RobotConfig &config = Comms::comms_layer.get_hive_data().config; @@ -48,7 +48,7 @@ void HelloRobot::init() { reference_map.emplace(config.states); target_state_map.emplace(config.states); // Temp ungoverned state hive_state_map_offset.emplace(config.states); // Hive offset state -*/ + // start the main loop watchdog watchdog.start(); } @@ -61,8 +61,8 @@ void HelloRobot::run() { stall_timer.start(); #ifdef PROFILER - /* - prof.begin("Telemetry"); + + prof.begin("Telemetry"); read_telemetry(); prof.end("Telemetry"); @@ -77,7 +77,7 @@ void HelloRobot::run() { prof.begin("Safety"); check_safety(); prof.end("Safety"); - */ + prof.begin("CLI"); process_cli(); prof.end("CLI"); From 97f7906ce150f30567599a30b4fe7bc64537eea1 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 16:50:04 -0400 Subject: [PATCH 24/41] adjusted comments to match new setup --- src/hello_robot.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 4798cff20..cfa49f144 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -262,7 +262,7 @@ void HelloRobot::process_cli() { if (millis() - last_redraw_time >= redraw_interval) { Serial.print("\033[H"); // Move cursor to top-left - // Loop through the array and draw the views in the exact order the user typed them + // Loop through the array and draw the views in the order the user typed them for (int i = 0; i < num_active_views; i++) { switch (active_views[i]) { case LiveMode::PROFILE_VIEW: @@ -325,9 +325,9 @@ void HelloRobot::process_cli() { // 3. NORMAL CLI PARSING // ========================================== /* Adding new commands is simple. - add if statement to cmd elseif block below + add command to command dictionary block below If it is live add to live mode enum and switch statement above - as well as to cmd elseif block below + as well as to view_dict lookup table in cmd_live function */ while (Serial.available() > 0) { char c = Serial.read(); From 8ac23fa90e332bfd252f719d431b77654bacd0c6 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 16:59:21 -0400 Subject: [PATCH 25/41] added comments --- src/hello_robot.hpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index d04838fd1..23b81c01e 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -140,13 +140,17 @@ class HelloRobot { // ========================================== // CLI Variables // ========================================== + /// @breif Collection of Live viewmodes enum class LiveMode { NONE, PROFILE_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE, SENSORS, HEARTBEAT }; - + /// @brief number of live views allowed at once static const uint8_t MAX_LIVE_VIEWS = 4; + /// @brief array of current live views LiveMode active_views[MAX_LIVE_VIEWS]; + /// @brief number of active live views uint8_t num_active_views = 0; - + /// @breif time since the live view was refreshed uint32_t last_redraw_time = 0; + /// @brief refresh rate in milliseconds uint32_t redraw_interval = 1000; /// @brief CLI Buffer char cli_buffer[64] = {0}; @@ -155,6 +159,13 @@ class HelloRobot { /// @brief flag for live CLI printing bool live_profiler_active = false; + /// @brief CLI ping function + void cmd_ping(); + /// @brief CLI help function + void cmd_help(); + /// @brief CLI live view function + void cmd_live(); + // ========================================== // Major Loop functions // ========================================== @@ -172,15 +183,13 @@ class HelloRobot { /// @brief Checks loop timing/safety constraints and writes to the CAN bus. void check_safety(); + + /// @brief Command line interface for live printing + void process_cli(); /// @brief LED hearbeat, feeds the watchdog, and ensures consistent loop time. void loop_timing(); - /// @brief Command line interface for live printing - void process_cli(); - void cmd_ping(); - void cmd_help(); - void cmd_live(); public: /** From 848ec82d8eaabc0feec5b3bdc37efde7381f47f6 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 17:03:06 -0400 Subject: [PATCH 26/41] misspeled brief --- src/hello_robot.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 23b81c01e..5df0d393d 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -140,7 +140,7 @@ class HelloRobot { // ========================================== // CLI Variables // ========================================== - /// @breif Collection of Live viewmodes + /// @brief Collection of Live viewmodes enum class LiveMode { NONE, PROFILE_VIEW, TRANSMITTER, ESTIMATED_STATE, TARGET_STATE, SENSORS, HEARTBEAT }; /// @brief number of live views allowed at once static const uint8_t MAX_LIVE_VIEWS = 4; @@ -148,7 +148,7 @@ class HelloRobot { LiveMode active_views[MAX_LIVE_VIEWS]; /// @brief number of active live views uint8_t num_active_views = 0; - /// @breif time since the live view was refreshed + /// @brief time since the live view was refreshed uint32_t last_redraw_time = 0; /// @brief refresh rate in milliseconds uint32_t redraw_interval = 1000; From 70bcba97ea2fef07703705d24b400ca1a88da326 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 17:19:19 -0400 Subject: [PATCH 27/41] added log command to CLI --- src/hello_robot.cpp | 27 ++++++++++++++++++++++++++- src/hello_robot.hpp | 3 ++- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 169be382a..7b0acb763 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -344,7 +344,8 @@ void HelloRobot::process_cli() { } commands[] = { {"ping", &HelloRobot::cmd_ping}, {"help", &HelloRobot::cmd_help}, - {"live", &HelloRobot::cmd_live} + {"live", &HelloRobot::cmd_live}, + {"log", &HelloRobot::cmd_log} }; // --- THE PARSER --- @@ -457,3 +458,27 @@ void HelloRobot::cmd_live() { Serial.println("Usage: live [prof] [tx] [sensors] [estimated_state] [target_state] [heartbeat]"); } } + +void HelloRobot::cmd_log() { + char* sys_tok = strtok(NULL, " "); + char* lvl_tok = strtok(NULL, " "); + + if (sys_tok) { + if (strcmp(sys_tok, "all") == 0) SystemLog.view_filter_sys = Subsystem::ALL; + else if (strcmp(sys_tok, "can") == 0) SystemLog.view_filter_sys = Subsystem::CAN; + else if (strcmp(sys_tok, "motors") == 0) SystemLog.view_filter_sys = Subsystem::MOTORS; + else if (strcmp(sys_tok, "sensors") == 0) SystemLog.view_filter_sys = Subsystem::SENSORS; + else if (strcmp(sys_tok, "est") == 0) SystemLog.view_filter_sys = Subsystem::ESTIMATOR; + else if (strcmp(sys_tok, "comms") == 0) SystemLog.view_filter_sys = Subsystem::COMMS; + } + + if (lvl_tok) { + if (strcmp(lvl_tok, "info") == 0) SystemLog.view_filter_level = LogLevel::INFO; + else if (strcmp(lvl_tok, "warn") == 0) SystemLog.view_filter_level = LogLevel::WARN; + else if (strcmp(lvl_tok, "error") == 0) SystemLog.view_filter_level = LogLevel::ERROR; + } + + Serial.printf("Log filter updated. Sys: %s | Level: %s\n", + sys_tok ? sys_tok : "UNCHANGED", + lvl_tok ? lvl_tok : "UNCHANGED"); +} diff --git a/src/hello_robot.hpp b/src/hello_robot.hpp index 5df0d393d..f93644253 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -165,7 +165,8 @@ class HelloRobot { void cmd_help(); /// @brief CLI live view function void cmd_live(); - + /// @brief CLI function to handle logging + void cmd_log(); // ========================================== // Major Loop functions // ========================================== From bd85f1d5b23faa4aeb63c0f6aa6cce8a9a482f05 Mon Sep 17 00:00:00 2001 From: EricV Date: Sun, 31 May 2026 18:11:26 -0400 Subject: [PATCH 28/41] converted cmd_log to lookup table and fixed some bugs --- src/hello_robot.cpp | 94 ++++++++++++++++++++++++++++++++-------- src/utils/system_log.cpp | 35 +++++++-------- src/utils/system_log.hpp | 4 +- 3 files changed, 97 insertions(+), 36 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 7b0acb763..872aec7f4 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -12,7 +12,7 @@ void HelloRobot::init() { // Configure the robot from comms data, which is filled on Hive. Serial.println("Configuring..."); - +/* Comms::comms_layer.configure(); const Cfg::RobotConfig &config = Comms::comms_layer.get_hive_data().config; @@ -48,7 +48,7 @@ void HelloRobot::init() { reference_map.emplace(config.states); target_state_map.emplace(config.states); // Temp ungoverned state hive_state_map_offset.emplace(config.states); // Hive offset state - +*/ // start the main loop watchdog watchdog.start(); } @@ -61,7 +61,7 @@ void HelloRobot::run() { stall_timer.start(); #ifdef PROFILER - +/* prof.begin("Telemetry"); read_telemetry(); prof.end("Telemetry"); @@ -77,7 +77,7 @@ void HelloRobot::run() { prof.begin("Safety"); check_safety(); prof.end("Safety"); - +*/ prof.begin("CLI"); process_cli(); prof.end("CLI"); @@ -87,8 +87,10 @@ void HelloRobot::run() { update_controls(); check_safety(); process_cli(); - #endif - loop_timing(); +#endif + prof.begin("loop_timing"); + loop_timing(); + prof.end("loop_timing"); } @@ -379,6 +381,9 @@ void HelloRobot::process_cli() { } void HelloRobot::cmd_ping() { Serial.println("pong! Robot is alive."); + prof.begin("Logging"); + SystemLog.error(Subsystem::MOTORS, "Overcurrent fault "); + prof.end("Logging"); } void HelloRobot::cmd_help() { @@ -463,21 +468,76 @@ void HelloRobot::cmd_log() { char* sys_tok = strtok(NULL, " "); char* lvl_tok = strtok(NULL, " "); + // --- DATA DICTIONARIES --- + struct SysMap { + const char* name; + Subsystem sys; + }; + static const SysMap sys_dict[] = { + {"all", Subsystem::ALL}, + {"can", Subsystem::CAN}, + {"motors", Subsystem::MOTORS}, + {"sensors", Subsystem::SENSORS}, + {"est", Subsystem::ESTIMATOR}, + {"comms", Subsystem::COMMS} + }; + + struct LvlMap { + const char* name; + LogLevel lvl; + }; + static const LvlMap lvl_dict[] = { + {"info", LogLevel::INFO}, + {"warn", LogLevel::WARN}, + {"error", LogLevel::ERROR} + }; + + bool error_found = false; + + // 1. Validate Subsystem (if provided) if (sys_tok) { - if (strcmp(sys_tok, "all") == 0) SystemLog.view_filter_sys = Subsystem::ALL; - else if (strcmp(sys_tok, "can") == 0) SystemLog.view_filter_sys = Subsystem::CAN; - else if (strcmp(sys_tok, "motors") == 0) SystemLog.view_filter_sys = Subsystem::MOTORS; - else if (strcmp(sys_tok, "sensors") == 0) SystemLog.view_filter_sys = Subsystem::SENSORS; - else if (strcmp(sys_tok, "est") == 0) SystemLog.view_filter_sys = Subsystem::ESTIMATOR; - else if (strcmp(sys_tok, "comms") == 0) SystemLog.view_filter_sys = Subsystem::COMMS; + bool found = false; + for (const auto& entry : sys_dict) { + if (strcmp(sys_tok, entry.name) == 0) { + SystemLog.view_filter_sys = entry.sys; + found = true; + break; + } + } + + if (!found) { + Serial.printf("Error: Unknown subsystem '%s'\n", sys_tok); + error_found = true; + } } - - if (lvl_tok) { - if (strcmp(lvl_tok, "info") == 0) SystemLog.view_filter_level = LogLevel::INFO; - else if (strcmp(lvl_tok, "warn") == 0) SystemLog.view_filter_level = LogLevel::WARN; - else if (strcmp(lvl_tok, "error") == 0) SystemLog.view_filter_level = LogLevel::ERROR; + + // 2. Validate Level (if provided and we haven't already failed) + if (lvl_tok && !error_found) { + bool found = false; + for (const auto& entry : lvl_dict) { + if (strcmp(lvl_tok, entry.name) == 0) { + SystemLog.view_filter_level = entry.lvl; + found = true; + break; + } + } + + if (!found) { + Serial.printf("Error: Unknown priority level '%s'\n", lvl_tok); + error_found = true; + } + } + + // 3. Check if an error occurred OR if the user just typed "log" with no arguments + if (error_found || (!sys_tok && !lvl_tok)) { + Serial.println("Usage: log [subsystem] [priority]"); + Serial.println(" Subsystems: all, can, motors, sensors, est, comms"); + Serial.println(" Priorities: info, warn, error"); + Serial.println(" Example: log motors warn"); + return; } + // 4. Success Output Serial.printf("Log filter updated. Sys: %s | Level: %s\n", sys_tok ? sys_tok : "UNCHANGED", lvl_tok ? lvl_tok : "UNCHANGED"); diff --git a/src/utils/system_log.cpp b/src/utils/system_log.cpp index a16a238d1..71dd956fe 100644 --- a/src/utils/system_log.cpp +++ b/src/utils/system_log.cpp @@ -58,7 +58,7 @@ void SystemLogger::push_message() { strncpy(messages[head].text, current_line, MAX_LINE_LEN); // 2. If CLI is closed, print immediately with colors! - if (!is_live_view_active) { + if (!is_live_view_active && should_show(messages[head].level, messages[head].sys)) { Serial.printf("[%7.2fs] %s[%s] %s\033[0m\n", messages[head].timestamp, level_to_color(messages[head].level), @@ -121,6 +121,22 @@ void SystemLogger::error(Subsystem sys, const char* format, ...) { line_length = len; push_message(); } + +bool SystemLogger::should_show(LogLevel lvl, Subsystem sys) { + // Only evaluate if the message meets the minimum requested priority level + if (lvl >= view_filter_level) { + // Rule 1: High priority (Warnings & Errors) ALWAYS pierce the subsystem filter + if (lvl >= LogLevel::WARN) { + return true; + } + // Rule 2: Standard INFO messages only show if they match the active subsystem + else if (view_filter_sys == Subsystem::ALL || sys == view_filter_sys) { + return true; + } + } + return false; +} + void SystemLogger::draw_dashboard_box() { Serial.println("============= SYSTEM EVENT LOG ============="); if (count == 0) { @@ -131,22 +147,7 @@ void SystemLogger::draw_dashboard_box() { uint8_t idx = (start + i) % LOG_HISTORY; LogEvent& ev = messages[idx]; - bool show = false; - - // FILTER LOGIC - // Only evaluate if the message meets the minimum requested priority level - if (ev.level >= view_filter_level) { - // Rule 1: High priority (Warnings & Errors) ALWAYS pierce the subsystem filter - if (ev.level >= LogLevel::WARN) { - show = true; - } - // Rule 2: Standard INFO messages only show if they match the active subsystem - else if (view_filter_sys == Subsystem::ALL || ev.sys == view_filter_sys) { - show = true; - } - } - - if (show) { + if (should_show(ev.level, ev.sys)) { Serial.printf(" [%7.2fs] %s[%s] %s\033[0m\n", ev.timestamp, level_to_color(ev.level), diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 6afd3cc92..f7a1b1638 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -66,8 +66,8 @@ class SystemLogger : public Print { void info(Subsystem sys, const char* format, ...); void warn(Subsystem sys, const char* format, ...); void error(Subsystem sys, const char* format, ...); - - + bool should_show(LogLevel lvl, Subsystem sys); + /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); }; From 6416c8b62e40185bb7744bef3a8d7c94db929f0c Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 2 Jun 2026 19:51:44 -0400 Subject: [PATCH 29/41] adjusted print statements in main loops to be system log statements --- src/hello_robot.cpp | 50 ++++++++++++++++++++--------------- src/utils/system_log.cpp | 56 +++++++++++++++++++++++----------------- src/utils/system_log.hpp | 13 ++++++---- 3 files changed, 69 insertions(+), 50 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 872aec7f4..0a063bd26 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -1,4 +1,5 @@ #include "hello_robot.hpp" +#include "system_log.hpp" #ifdef PROFILER Profiler prof; #endif @@ -12,7 +13,7 @@ void HelloRobot::init() { // Configure the robot from comms data, which is filled on Hive. Serial.println("Configuring..."); -/* + Comms::comms_layer.configure(); const Cfg::RobotConfig &config = Comms::comms_layer.get_hive_data().config; @@ -48,7 +49,7 @@ void HelloRobot::init() { reference_map.emplace(config.states); target_state_map.emplace(config.states); // Temp ungoverned state hive_state_map_offset.emplace(config.states); // Hive offset state -*/ + // start the main loop watchdog watchdog.start(); } @@ -61,7 +62,7 @@ void HelloRobot::run() { stall_timer.start(); #ifdef PROFILER -/* + prof.begin("Telemetry"); read_telemetry(); prof.end("Telemetry"); @@ -77,7 +78,7 @@ void HelloRobot::run() { prof.begin("Safety"); check_safety(); prof.end("Safety"); -*/ + prof.begin("CLI"); process_cli(); prof.end("CLI"); @@ -88,11 +89,7 @@ void HelloRobot::run() { check_safety(); process_cli(); #endif - prof.begin("loop_timing"); - loop_timing(); - prof.end("loop_timing"); - - + loop_timing(); } } @@ -141,7 +138,7 @@ void HelloRobot::process_behaviors() { // clear the request Comms::comms_layer.get_hive_data().override_state_data.active = false; - SystemLog.printf("Overriding state with hive state\n"); + SystemLog.info(Subsystem::GENERAL,"Overriding state with hive state\n"); hive_state_map_offset->from_comms_packet(Comms::comms_layer.get_hive_data().override_state_data.state); *estimated_state_map = *hive_state_map_offset; @@ -156,7 +153,7 @@ void HelloRobot::update_controls() { float current_feed = (*estimated_state_map)[Cfg::StateName::Feeder].get_position(); float target_feed = (*target_state_map)[Cfg::StateName::Feeder].get_position(); if ((feed - current_feed > 2 && transmitter_manager.is_teensy_mode()) || (target_feed - current_feed > 2 && transmitter_manager.is_hive_mode())) { - SystemLog.printf("Feeder is lowkey jammed. current ball count: %f, feed: %f, hive target: %f\n", (*estimated_state_map)[Cfg::StateName::Feeder].get_position(), feed, (*target_state_map)[Cfg::StateName::Feeder].get_position()); + SystemLog.error(Subsystem::GENERAL,"Feeder is lowkey jammed. current ball count: %f, feed: %f, hive target: %f\n", (*estimated_state_map)[Cfg::StateName::Feeder].get_position(), feed, (*target_state_map)[Cfg::StateName::Feeder].get_position()); feed = current_feed + 1; governor->set_position_reference(Cfg::StateName::Feeder, feed); } @@ -194,13 +191,13 @@ void HelloRobot::check_safety() { // zero the can bus just in case can.issue_safety_mode(); - SystemLog.printf("Slow loop with dt: %f, slow loop count %d\n", dt, slow_loop_counter); + SystemLog.error(Subsystem::GENERAL,"Slow loop with dt: %f, slow loop count %d\n", dt, slow_loop_counter); // mark this as a slow loop to trigger safety mode is_slow_loop = true; if (last_loop_slow) { slow_loop_counter++; if (slow_loop_counter > 10) { - SystemLog.printf("Kowabunga bitches\n"); + SystemLog.error("Kowabunga bitches\n"); reset_teensy(); } } else { @@ -223,7 +220,7 @@ void HelloRobot::check_safety() { if (not_safety_mode) { // SAFETY OFF can.write(); - // Serial.printf("Can write\n"); + SystemLog.info(Subsystem::CAN,"Can write\n"); } else { // SAFETY ON // TODO: Reset all controller integrators here @@ -381,9 +378,6 @@ void HelloRobot::process_cli() { } void HelloRobot::cmd_ping() { Serial.println("pong! Robot is alive."); - prof.begin("Logging"); - SystemLog.error(Subsystem::MOTORS, "Overcurrent fault "); - prof.end("Logging"); } void HelloRobot::cmd_help() { @@ -413,6 +407,21 @@ void HelloRobot::cmd_help() { Serial.println(" estimated_state : The robot's current estimated state map"); Serial.println(" target_state : The robot's current target state map"); Serial.println(" heartbeat : The main loop counter (loopc)"); + Serial.println(); + Serial.println(" log [subsystem] [priority]"); + Serial.println(" Filters the system event log."); + Serial.println(" High-priority messages (Errors) will always bypass the subsystem filter."); + Serial.println(" Typing 'log' with no arguments displays the syntax menu."); + Serial.println(); + Serial.println(" Available subsystems:"); + Serial.println(" all, can, motors, sensors, est, comms"); + Serial.println(); + Serial.println(" Available priorities (minimum level to show):"); + Serial.println(" info, warn, error"); + Serial.println(); + Serial.println(" Examples:"); + Serial.println(" log motors warn : Shows motor warnings/errors, and all other system errors"); + Serial.println(" log all info : Resets the filter to show absolutely everything"); Serial.println(); Serial.println(" help"); Serial.println(" Displays this manual."); @@ -494,7 +503,7 @@ void HelloRobot::cmd_log() { bool error_found = false; - // 1. Validate Subsystem (if provided) + // Check Subsystem (if provided) if (sys_tok) { bool found = false; for (const auto& entry : sys_dict) { @@ -511,7 +520,7 @@ void HelloRobot::cmd_log() { } } - // 2. Validate Level (if provided and we haven't already failed) + // Check Priority Level (if provided) if (lvl_tok && !error_found) { bool found = false; for (const auto& entry : lvl_dict) { @@ -528,7 +537,7 @@ void HelloRobot::cmd_log() { } } - // 3. Check if an error occurred OR if the user just typed "log" with no arguments + // Check if log statement was written correctly if (error_found || (!sys_tok && !lvl_tok)) { Serial.println("Usage: log [subsystem] [priority]"); Serial.println(" Subsystems: all, can, motors, sensors, est, comms"); @@ -537,7 +546,6 @@ void HelloRobot::cmd_log() { return; } - // 4. Success Output Serial.printf("Log filter updated. Sys: %s | Level: %s\n", sys_tok ? sys_tok : "UNCHANGED", lvl_tok ? lvl_tok : "UNCHANGED"); diff --git a/src/utils/system_log.cpp b/src/utils/system_log.cpp index 71dd956fe..86998a74d 100644 --- a/src/utils/system_log.cpp +++ b/src/utils/system_log.cpp @@ -74,52 +74,60 @@ void SystemLogger::push_message() { current_level = LogLevel::INFO; // Reset to default current_sys = Subsystem::GENERAL; // Reset to default } -// This bypasses the write() character loop and instantly builds a struct -void SystemLogger::info(Subsystem sys, const char* format, ...) { +void SystemLogger::log_format(LogLevel lvl, Subsystem sys, const char* format, va_list args) { char temp[MAX_LINE_LEN]; - va_list args; - va_start(args, format); vsnprintf(temp, MAX_LINE_LEN, format, args); - va_end(args); // Strip trailing newlines so it formats perfectly size_t len = strlen(temp); while(len > 0 && (temp[len-1] == '\n' || temp[len-1] == '\r')) temp[--len] = '\0'; - set_context(LogLevel::INFO, sys); + set_context(lvl, sys); strncpy(current_line, temp, MAX_LINE_LEN); line_length = len; push_message(); } -void SystemLogger::warn(Subsystem sys, const char* format, ...) { - char temp[MAX_LINE_LEN]; +// --- Info Overloads --- +void SystemLogger::info(Subsystem sys, const char* format, ...) { va_list args; va_start(args, format); - vsnprintf(temp, MAX_LINE_LEN, format, args); + log_format(LogLevel::INFO, sys, format, args); va_end(args); - size_t len = strlen(temp); - while(len > 0 && (temp[len-1] == '\n' || temp[len-1] == '\r')) temp[--len] = '\0'; +} +void SystemLogger::info(const char* format, ...) { + va_list args; + va_start(args, format); + log_format(LogLevel::INFO, Subsystem::GENERAL, format, args); + va_end(args); +} - set_context(LogLevel::WARN, sys); - strncpy(current_line, temp, MAX_LINE_LEN); - line_length = len; - push_message(); +// --- Warn Overloads --- +void SystemLogger::warn(Subsystem sys, const char* format, ...) { + va_list args; + va_start(args, format); + log_format(LogLevel::WARN, sys, format, args); + va_end(args); +} +void SystemLogger::warn(const char* format, ...) { + va_list args; + va_start(args, format); + log_format(LogLevel::WARN, Subsystem::GENERAL, format, args); + va_end(args); } +// --- Error Overloads --- void SystemLogger::error(Subsystem sys, const char* format, ...) { - char temp[MAX_LINE_LEN]; va_list args; va_start(args, format); - vsnprintf(temp, MAX_LINE_LEN, format, args); + log_format(LogLevel::ERROR, sys, format, args); + va_end(args); +} +void SystemLogger::error(const char* format, ...) { + va_list args; + va_start(args, format); + log_format(LogLevel::ERROR, Subsystem::GENERAL, format, args); va_end(args); - size_t len = strlen(temp); - while(len > 0 && (temp[len-1] == '\n' || temp[len-1] == '\r')) temp[--len] = '\0'; - - set_context(LogLevel::ERROR, sys); - strncpy(current_line, temp, MAX_LINE_LEN); - line_length = len; - push_message(); } bool SystemLogger::should_show(LogLevel lvl, Subsystem sys) { diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index f7a1b1638..283761264 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -39,7 +39,9 @@ class SystemLogger : public Print { Subsystem current_sys = Subsystem::GENERAL; /// @brief handles whether we print to dashboard or direct to serial void push_message(); - + /// @brief string formatter so we don't repeat code + void log_format(LogLevel lvl, Subsystem sys, const char* format, va_list args); + bool should_show(LogLevel lvl, Subsystem sys); public: /// @brief flag for live printing from CLI bool is_live_view_active = false; @@ -59,15 +61,16 @@ class SystemLogger : public Print { /// @return the message size_t write(const uint8_t *buffer, size_t size) override; - // --- NEW: Context setter for multiline Print() blocks --- + void set_context(LogLevel lvl, Subsystem sys); - // --- NEW: Fast, formatted logger helpers --- void info(Subsystem sys, const char* format, ...); void warn(Subsystem sys, const char* format, ...); void error(Subsystem sys, const char* format, ...); - bool should_show(LogLevel lvl, Subsystem sys); - + + void info(const char* format, ...); + void warn(const char* format, ...); + void error(const char* format, ...); /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); }; From e61d080c0f14cd7ed6f38d875d8b798d8feb36fc Mon Sep 17 00:00:00 2001 From: EricV Date: Mon, 8 Jun 2026 18:08:12 -0400 Subject: [PATCH 30/41] added lookup table to robot state_map print statement so that each state_map is called by its actual name instead of a number --- src/controls/robot_state_map.cpp | 51 ++++++++++++++++++++++++++++---- 1 file changed, 46 insertions(+), 5 deletions(-) diff --git a/src/controls/robot_state_map.cpp b/src/controls/robot_state_map.cpp index 5e8d260f7..e7faf6f0a 100644 --- a/src/controls/robot_state_map.cpp +++ b/src/controls/robot_state_map.cpp @@ -46,8 +46,49 @@ void RobotStateMap::from_comms_packet(State::Raw robot_state_array[NUM_STATES]) void RobotStateMap::print() { Serial.println("RobotStateMap:"); - for (const auto& [state_name, state] : robot_state) { - Serial.printf("\tStateName: %lu, Position: %f, Velocity: %f, Acceleration: %f\n", static_cast(state_name), state.get_position(), state.get_velocity(), state.get_acceleration()); - Serial.printf("\t\tPosition limits: [%f, %f]\n", state.config().reference_limits.position.min, state.config().reference_limits.position.max); - } -} \ No newline at end of file + + auto state_to_str = [](Cfg::StateName name) -> const char* { + switch (name) { + // Replace these with your actual enum values! + case Cfg::StateName::UnsetStateName: + return "UnsetStateName"; + case Cfg::StateName::ChassisX: + return "X"; + case Cfg::StateName::ChassisY: + return "Y"; + case Cfg::StateName::ChassisHeading: + return "Z"; + case Cfg::StateName::GimbalYaw: + return "YAW"; + case Cfg::StateName::GimbalPitch: + return "PITCH"; + case Cfg::StateName::Flywheels: + return "Flywheels"; + case Cfg::StateName::Feeder: + return "Feeder"; + case Cfg::StateName::LowerFeeder: + return "LowerFeeder"; + case Cfg::StateName::StructPadding: + return "StructPadding"; + case Cfg::StateName::StateNameCount: + return "StateNameCount"; + default: + return "UNKNOWN"; + } + }; + + for (const auto& [state_name, state] : robot_state) { + + // Use %-12s to force the string to 12 characters, erasing terminal ghosting + // Use %8.3f to keep the decimals perfectly aligned in a column + Serial.printf("\tState: %-12s | Pos: %8.3f | Vel: %8.3f | Acc: %8.3f\n", + state_to_str(state_name), + state.get_position(), + state.get_velocity(), + state.get_acceleration()); + + Serial.printf("\t\tPos Limits: [%.2f, %.2f]\n", + state.config().reference_limits.position.min, + state.config().reference_limits.position.max); + } +} From a2b2824c326ecd7c45406bea0da9437e5c80907d Mon Sep 17 00:00:00 2001 From: EricV Date: Fri, 28 Aug 2026 16:17:54 -0600 Subject: [PATCH 31/41] fixed debug and release make flags --- Makefile | 42 +++++++++++++++++++----------------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/Makefile b/Makefile index 34910b37a..9dcc39891 100644 --- a/Makefile +++ b/Makefile @@ -1,9 +1,17 @@ -# Optional features: PROFILER, COMMS_DEBUG, REF_SYSTEM_DEBUG, CAN_MANAGER_DEBUG -# Set them on the line below, e.g. FEATURE_DEFINES ?= -DPROFILER -DCOMMS_DEBUG -# Rebuild from scratch after editing this line: -# make clean -# make build -FEATURE_DEFINES ?= +BUILD_TYPE ?= release +BUILD_BASE_DIR := build + +ifneq ($(filter debug,$(MAKECMDGOALS)),) + BUILD_TYPE := debug + FEATURE_DEFINES += -DPROFILER -DEBUG +endif + +ifneq ($(filter release,$(MAKECMDGOALS)),) + BUILD_TYPE := release +endif + +BUILD_DIR := $(BUILD_BASE_DIR)/$(BUILD_TYPE) +TOOLS_DIR := tools # Set to 1 to disassemble every object file alongside it, for inspecting a # single translation unit. @@ -15,21 +23,6 @@ DUMP_OBJS ?= 0 JOBS ?= $(shell sysctl -n hw.ncpu 2>/dev/null || nproc 2>/dev/null || echo 1) MAKEFLAGS += -j$(JOBS) -BUILD_DIR := build -TOOLS_DIR := tools - -ifneq ($(filter debug,$(MAKECMDGOALS)),) - BUILD_DIR := ./build/debug - PROFILER_FLAG := -DPROFILER -endif - -ifneq ($(filter release,$(MAKECMDGOALS)),) - BUILD_DIR := ./build/release -endif - -# Tools directory -TOOLS_DIR := ./tools - TARGET := firmware TARGET_ELF := $(BUILD_DIR)/$(TARGET).elf TARGET_HEX := $(BUILD_DIR)/$(TARGET).hex @@ -113,11 +106,14 @@ GIT_SCRAPER_SRC = $(TOOLS_DIR)/git_scraper.cpp GIT_SCRAPER_BIN = $(BUILD_DIR)/git_scraper -.PHONY: build dump docs clean upload install gdb monitor kill restart help clangd git_scraper +.PHONY: build debug release dump docs clean upload install gdb monitor kill restart help clangd git_scraper build: $(TARGET_HEX) +debug: build + +release: build dump: $(TARGET_DUMP) @@ -164,7 +160,7 @@ docs: build clean: - rm -rf $(BUILD_DIR) + rm -rf $(BUILD_BASE_DIR) rm -f compile_commands.json # Include the dependency files to manage header file dependencies From f1e3bc54ab859dad742bd9f84de12e822d366935 Mon Sep 17 00:00:00 2001 From: EricV Date: Fri, 28 Aug 2026 16:18:24 -0600 Subject: [PATCH 32/41] fixed typo --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 9dcc39891..605d85370 100644 --- a/Makefile +++ b/Makefile @@ -3,7 +3,7 @@ BUILD_BASE_DIR := build ifneq ($(filter debug,$(MAKECMDGOALS)),) BUILD_TYPE := debug - FEATURE_DEFINES += -DPROFILER -DEBUG + FEATURE_DEFINES += -DPROFILER endif ifneq ($(filter release,$(MAKECMDGOALS)),) From f6cfd732c72404333bacaf4f2d743707887b15c3 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 29 Aug 2026 16:16:37 -0600 Subject: [PATCH 33/41] no longer read out loop --- src/hello_robot.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 653a115a3..134214253 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -124,10 +124,6 @@ void HelloRobot::read_telemetry() { sensor_manager.read(); sensor_manager.send_to_comms(); - // print loopc every second to verify it is still alive - if (loopc % 1000 == 0) { - Serial.println(loopc); - } } void HelloRobot::process_behaviors() { // manual controls on firmware From 7770dd9a176b9a82be3fe6f3429bd15c50e75744 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 29 Aug 2026 17:46:30 -0600 Subject: [PATCH 34/41] fixed system_log include header --- src/hello_robot.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index db420b886..9b9c2870c 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -1,5 +1,4 @@ #include "hello_robot.hpp" -#include "system_log.hpp" #ifdef PROFILER Profiler prof; #endif From 99634c4c9b6605b690ce43a2dfd2a857e3762bf8 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 29 Aug 2026 17:56:44 -0600 Subject: [PATCH 35/41] cleaning unneccesary stuff --- src/hello_robot.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 9b9c2870c..87ed3a27b 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -149,7 +149,6 @@ void HelloRobot::process_behaviors() { void HelloRobot::update_controls() { // step estimates and construct estimated state estimator_manager.step(*estimated_state_map, override_request); - // estimated_state_map.print(); noInterrupts(); *estimated_state_map_interrupt_safe = *estimated_state_map; From f874e834c458d28644144f11eb232258ba9c8dc8 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 29 Aug 2026 18:28:51 -0600 Subject: [PATCH 36/41] added doxygen comments to system_log header --- src/utils/system_log.hpp | 49 ++++++++++++++++++++++++++++++---------- 1 file changed, 37 insertions(+), 12 deletions(-) diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 283761264..d653aa2d5 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -34,19 +34,31 @@ class SystemLogger : public Print { /// @brief length of print line in buffer uint8_t line_length = 0; - // Context for standard Print() calls + /// @brief Context for standard Print() calls LogLevel current_level = LogLevel::INFO; + /// @brief Subsystem for standard print call Subsystem current_sys = Subsystem::GENERAL; /// @brief handles whether we print to dashboard or direct to serial void push_message(); /// @brief string formatter so we don't repeat code - void log_format(LogLevel lvl, Subsystem sys, const char* format, va_list args); + /// @param lvl is the urgency level of log + /// @param sys is the subsystem + void log_format(LogLevel lvl, Subsystem sys, const char *format, va_list args); + /// @brief set context level and subsystem of interest for print statement + /// @param lvl is the urgency level of log + /// @param sys is the subsystem + void set_context(LogLevel lvl, Subsystem sys); + /// @brief logic for determing wether a message should print or not + /// @param lvl is the urgency level of log + /// @param sys is the subsystem + /// @return whether we show the message or not bool should_show(LogLevel lvl, Subsystem sys); public: /// @brief flag for live printing from CLI bool is_live_view_active = false; - // Dashboard Filters (Defaults to showing everything) + /// @brief Dashboard System Filters (Defaults to showing everything) Subsystem view_filter_sys = Subsystem::ALL; + /// @brief Dashboard Level Filters (Defaults to showing everything) LogLevel view_filter_level = LogLevel::INFO; @@ -60,16 +72,29 @@ class SystemLogger : public Print { /// @param size of message /// @return the message size_t write(const uint8_t *buffer, size_t size) override; - - - void set_context(LogLevel lvl, Subsystem sys); - - void info(Subsystem sys, const char* format, ...); - void warn(Subsystem sys, const char* format, ...); + /// @brief buffer info level message into queue + /// @param sys is the subsytem that will be coupled with the message + /// @param format is any printf variadic formatting information + void info(Subsystem sys, const char *format, ...); + /// @brief buffer warn level message into queue + /// @param sys is the subsytem that will be coupled with the message + /// @param format is any printf variadic formatting information + void warn(Subsystem sys, const char *format, ...); + /// @brief buffer error level message into queue + /// @param sys is the subsytem that will be coupled with the message + /// @param format is any printf variadic formatting information void error(Subsystem sys, const char* format, ...); - - void info(const char* format, ...); - void warn(const char* format, ...); + /// @brief buffer info level message into queue + /// @param format is any printf variadic formatting information + /// @note defaults to general subsystem + void info(const char *format, ...); + /// @brief buffer warn level message into queue + /// @param format is any printf variadic formatting information + /// @note defaults to general subsystem + void warn(const char *format, ...); + /// @brief buffer error level message into queue + /// @param format is any printf variadic formatting information + /// @note defaults to general subsystem void error(const char* format, ...); /// @brief draws dashboard for live prints from CLI void draw_dashboard_box(); From 2fb33189ea85ee22da854de573e19000694b6292 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 29 Aug 2026 18:38:06 -0600 Subject: [PATCH 37/41] more comments --- src/utils/system_log.hpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index d653aa2d5..9df331538 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -4,12 +4,16 @@ enum class LogLevel { INFO, WARN, ERROR }; /// @brief List of robot subsystems enum class Subsystem { ALL, GENERAL, CAN, MOTORS, SENSORS, ESTIMATOR, COMMS , REF}; - +/// @brief Event log struct which holds all relevent data about potential print struct LogEvent { + /// @brief current loop count float timestamp; + /// @brief Priority Level LogLevel level; + /// @brief Message's subsystem Subsystem sys; - char text[80]; // Max length of message + /// @breif contains message with max length 80 characters + char text[80]; }; /// @brief Serial wrapper for handling print statements @@ -22,7 +26,6 @@ class SystemLogger : public Print { /// @brief max length including the timestamp static const int MAX_STORED_LEN = 100; /// @brief message circuluar buffer - //char messages[LOG_HISTORY][MAX_LINE_LEN] = {{0}}; LogEvent messages[LOG_HISTORY]; /// @brief start of the message uint8_t head = 0; @@ -43,6 +46,8 @@ class SystemLogger : public Print { /// @brief string formatter so we don't repeat code /// @param lvl is the urgency level of log /// @param sys is the subsystem + /// @param format is the message + /// @param args is the variadic formatting options void log_format(LogLevel lvl, Subsystem sys, const char *format, va_list args); /// @brief set context level and subsystem of interest for print statement /// @param lvl is the urgency level of log From a829ce9a01cff8537918af0e25c97ba9cea25f0b Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 29 Aug 2026 18:39:08 -0600 Subject: [PATCH 38/41] final comment --- src/utils/system_log.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 9df331538..1cb13fb44 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -12,7 +12,7 @@ struct LogEvent { LogLevel level; /// @brief Message's subsystem Subsystem sys; - /// @breif contains message with max length 80 characters + /// @brief contains message with max length 80 characters char text[80]; }; From a54d4b9203bd773ad64e17479e596bcc8848d665 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 1 Sep 2026 15:09:16 -0600 Subject: [PATCH 39/41] changed a heckin lot of print statements to systemlog statements --- src/comms/comms_layer.cpp | 23 +++++----- src/comms/ethernet_comms.cpp | 29 ++++++------ src/controls/controller.cpp | 5 ++- src/controls/controller.hpp | 6 ++- src/controls/controller_manager.cpp | 7 +-- src/controls/estimator.cpp | 11 ++--- src/sensors/RefSystem.cpp | 69 +++++++++++++++-------------- src/sensors/buff_encoder.cpp | 13 +++--- src/sensors/can/GIM.cpp | 3 +- src/sensors/can/MG8016EI6.cpp | 5 ++- src/sensors/can/SDC104.cpp | 8 ++-- src/sensors/can/can_manager.cpp | 3 +- src/sensors/transmitter/ET16S.cpp | 5 ++- src/sensors/transmitter/dr16.cpp | 9 ++-- src/utils/system_log.cpp | 3 +- src/utils/system_log.hpp | 2 +- 16 files changed, 109 insertions(+), 92 deletions(-) diff --git a/src/comms/comms_layer.cpp b/src/comms/comms_layer.cpp index 2ed3a6367..b02e4032e 100644 --- a/src/comms/comms_layer.cpp +++ b/src/comms/comms_layer.cpp @@ -1,4 +1,5 @@ #include "comms_layer.hpp" +#include "utils/system_log.hpp" #include "comms/data/configuration_status_data.hpp" #include "comms/data/sendable.hpp" @@ -20,30 +21,30 @@ namespace Comms { CommsLayer comms_layer; CommsLayer::CommsLayer() { - Serial.printf("CommsLayer: constructed\n"); + SystemLog.info(Subsystem::COMMS,"CommsLayer: constructed\n"); }; CommsLayer::~CommsLayer() { - Serial.printf("CommsLayer: destructed\n"); + SystemLog.info(Subsystem::COMMS,"CommsLayer: destructed\n"); }; int CommsLayer::init() { - Serial.printf("CommsLayer: initializing\n"); + SystemLog.info(Subsystem::COMMS,"CommsLayer: initializing\n"); // hid failing is a fatal error bool hid_init = initialize_hid(); if (!hid_init) { - Serial.printf("CommsLayer: HIDComms init failed\n"); + SystemLog.error(Subsystem::COMMS,"CommsLayer: HIDComms init failed\n"); return -1; } // ethernet init failing is not a fatal error bool ethernet_init = initialize_ethernet(); if (!ethernet_init) { - Serial.printf("CommsLayer: EthernetComms init failed\n"); + SystemLog.info(Subsystem::COMMS,"CommsLayer: EthernetComms init failed\n"); } - Serial.printf("CommsLayer: initialized\n"); + SystemLog.info(Subsystem::COMMS,"CommsLayer: initialized\n"); return 0; }; @@ -63,7 +64,7 @@ void CommsLayer::queue_data(CommsData* data) { case PhysicalMedium::HID: if (!is_hid_connected()) { // discard attempt to send - Serial.printf("Attempting to re-route %s to HID but HID is not connected\n", to_string(data->type_label).c_str()); + SystemLog.warn(Subsystem::COMMS,"Attempting to re-route %s to HID but HID is not connected\n", to_string(data->type_label).c_str()); break; } m_hid_payload.add(data); @@ -75,7 +76,7 @@ void CommsLayer::queue_data(CommsData* data) { break; } else if (data->size > HID_PACKET_PAYLOAD_SIZE) { // discard attempt to send - Serial.printf("Attempting to re-route %s to HID but packet is too large\n", to_string(data->type_label).c_str()); + SystemLog.warn(Subsystem::COMMS,"Attempting to re-route %s to HID but packet is too large\n", to_string(data->type_label).c_str()); break; } @@ -169,19 +170,19 @@ void CommsLayer::configure() { int time = millis(); Sendable config_status_sendable; while (!m_hive_data.config.config_start.num_config_sections != 0) { - Serial.printf("Waiting for config start packet... time since start: %d ms\n", millis() - time); + SystemLog.info(Subsystem::COMMS,"Waiting for config start packet... time since start: %d ms\n", millis() - time); config_status_sendable.data.is_configured = 0; config_status_sendable.send_to_comms(); run(); config_loop_timer.delay_micros(5000); } - Serial.printf("Config start packet received, expecting %d config sections\n", m_hive_data.config.config_start.num_config_sections); + SystemLog.info(Subsystem::COMMS,"Config start packet received, expecting %d config sections\n", m_hive_data.config.config_start.num_config_sections); while(!m_hive_data.config.is_configured()) { config_status_sendable.data.ready_for_config = 1; config_status_sendable.send_to_comms(); run(); - Serial.printf("Config: received %d of %d sections\n", m_hive_data.config.num_sections_received, m_hive_data.config.config_start.num_config_sections); + SystemLog.info(Subsystem::COMMS,"Config: received %d of %d sections\n", m_hive_data.config.num_sections_received, m_hive_data.config.config_start.num_config_sections); config_loop_timer.delay_micros(5000); } } diff --git a/src/comms/ethernet_comms.cpp b/src/comms/ethernet_comms.cpp index 61ea3cb9c..29534d92b 100644 --- a/src/comms/ethernet_comms.cpp +++ b/src/comms/ethernet_comms.cpp @@ -1,4 +1,5 @@ #include "ethernet_comms.hpp" +#include "utils/system_log.hpp" namespace Comms { @@ -6,18 +7,18 @@ bool EthernetComms::init(uint32_t data_rate) { // begin the ethernet library and verify the status of the line uint8_t ethernet_status = qn::Ethernet.begin(m_teensy_ip, m_teensy_netmask, m_teensy_gateway); if (!ethernet_status) { - Serial.printf("EthernetComms: Failed to start Ethernet!\n"); + SystemLog.warn(Subsystem::COMMS,"EthernetComms: Failed to start Ethernet!\n"); // check if this teensy even has ethernet hardware support qn::EthernetHardwareStatus hw_status = qn::Ethernet.hardwareStatus(); if (hw_status != qn::EthernetHardwareStatus::EthernetTeensy41) - Serial.printf("EthernetComms: Teensy Ethernet hardware not present!\n"); + SystemLog.warn(Subsystem::COMMS,"EthernetComms: Teensy Ethernet hardware not present!\n"); return false; } else { #ifdef COMMS_DEBUG - Serial.printf("EthernetComms: Ethernet started!\n"); - Serial.printf("EthernetComms: IP: %u.%u.%u.%u:%u\n", qn::Ethernet.localIP()[0], qn::Ethernet.localIP()[1], qn::Ethernet.localIP()[2], qn::Ethernet.localIP()[3], m_teensy_port); + SystemLog.info(Subsystem::COMMS,"EthernetComms: Ethernet started!\n"); + SystemLog.info(Subsystem::COMMS,"EthernetComms: IP: %u.%u.%u.%u:%u\n", qn::Ethernet.localIP()[0], qn::Ethernet.localIP()[1], qn::Ethernet.localIP()[2], qn::Ethernet.localIP()[3], m_teensy_port); #endif } @@ -25,12 +26,12 @@ bool EthernetComms::init(uint32_t data_rate) { uint8_t udp_status = m_udp_server.begin(m_teensy_port); // this failing is extremely bad. Either the Teensy is out of memory, or the hardware is broken if (!udp_status) { - Serial.printf("EthernetComms: UDP server failed to start!\n"); + SystemLog.warn(Subsystem::COMMS,"EthernetComms: UDP server failed to start!\n"); return false; } else { #ifdef COMMS_DEBUG - Serial.printf("EthernetComms: UDP server started!\n"); + SystemLog.info(Subsystem::COMMS,"EthernetComms: UDP server started!\n"); #endif } @@ -57,8 +58,8 @@ bool EthernetComms::init(uint32_t data_rate) { // this is generally a problem with the Linux side, not the Teensy // (the ethernet cable might be disconnected or the Linux side is not running) if (micros() - warmup_start >= m_warmup_timeout) { - Serial.printf("EthernetComms: UDP server warmup failed!\n"); - Serial.printf("EthernetComms: Is the ethernet cable connected?\n"); + SystemLog.warn(Subsystem::COMMS,"EthernetComms: UDP server warmup failed!\n"); + SystemLog.warn(Subsystem::COMMS,"EthernetComms: Is the ethernet cable connected?\n"); return false; } @@ -66,7 +67,7 @@ bool EthernetComms::init(uint32_t data_rate) { } // ethernet is now setup and udp server is online - Serial.printf("EthernetComms: Ethernet online!\n"); + SystemLog.info(Subsystem::COMMS,"EthernetComms: Ethernet online!\n"); // set the initialized flag m_initialized = true; @@ -94,7 +95,7 @@ bool EthernetComms::send_packet(EthernetPacket& packet) { if (!send_status) { // log the fail, this almost always happens when the udp server is not "warmed up" #if defined(COMMS_DEBUG) - Serial.printf("EthernetComms: Send fail\n"); + SystemLog.info(Subsystem::COMMS,"EthernetComms: Send fail\n"); #endif return false; } else { @@ -120,7 +121,7 @@ bool EthernetComms::recv_packet(EthernetPacket& packet) { } else if (current_buffer_size != Comms::ETHERNET_PACKET_MAX_SIZE) { // half-read, log as a failure #if defined(COMMS_DEBUG) - Serial.printf("EthernetComms: Recv fail: %d\n", current_buffer_size); + SystemLog.info(Subsystem::COMMS,"EthernetComms: Recv fail: %d\n", current_buffer_size); #endif return false; } else { @@ -129,7 +130,7 @@ bool EthernetComms::recv_packet(EthernetPacket& packet) { // this should never happen, but sanity check if (packet_data == NULL) { #if defined(COMMS_DEBUG) - Serial.printf("EthernetComms: Recv data NULL\n"); + SystemLog.info(Subsystem::COMMS,"EthernetComms: Recv data NULL\n"); #endif } @@ -157,7 +158,7 @@ void EthernetComms::check_connection() { // if the last packet was received too long ago, timeout the connection if (micros() - m_last_recv_time > m_connection_timeout) { // this check ensures this is only printed once - if (m_connected) Serial.printf("EthernetComms: Connection lost!\n"); + if (m_connected) SystemLog.warn(Subsystem::COMMS,"EthernetComms: Connection lost!\n"); // mark the connection as disconnected m_connected = false; } else { @@ -167,4 +168,4 @@ void EthernetComms::check_connection() { return; } -} // namespace Comms \ No newline at end of file +} // namespace Comms diff --git a/src/controls/controller.cpp b/src/controls/controller.cpp index f8c1d283d..a36f0577e 100644 --- a/src/controls/controller.cpp +++ b/src/controls/controller.cpp @@ -1,6 +1,7 @@ #include "controller.hpp" #include "sensors/can/motor.hpp" #include "sensors/RefSystem.hpp" +#include "utils/system_log.hpp" namespace { /// @brief Unwrap a potentially wrapped error value to maintain continuity across wrap boundaries. @@ -280,7 +281,7 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat drive_motors[i]->write_motor_torque(motor_outputs[i]); } } else { - Serial.printf("governor type not used for xdrive controller"); + SystemLog.error(Subsystem::Controls,"governor type not used for xdrive controller"); } } @@ -527,4 +528,4 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es near_feeder_motor->write_motor_torque(lower_output); far_feeder_motor->write_motor_torque(-lower_output); -} \ No newline at end of file +} diff --git a/src/controls/controller.hpp b/src/controls/controller.hpp index b9392815d..63044e763 100644 --- a/src/controls/controller.hpp +++ b/src/controls/controller.hpp @@ -5,6 +5,8 @@ #include "sensors/can/motor.hpp" #include "utils/safety.hpp" #include "utils/timing.hpp" +#include "utils/system_log.hpp" + #include "sensors/can/can_manager.hpp" #include "robot_state_map.hpp" #include "reference_governor.hpp" @@ -540,7 +542,7 @@ struct LowerFeederController : public Controller { bool found = false; for (auto& state: state_config) { if (state.name == upper_feeder_position_state) { - Serial.printf("state config, reference limits velocity: min %f, max %f\n", state.reference_limits.velocity.min, state.reference_limits.velocity.max); + SystemLog.info(Subsystem::Controls,"state config, reference limits velocity: min %f, max %f\n", state.reference_limits.velocity.min, state.reference_limits.velocity.max); upper_feeder_reference_state.get_state_map().emplace(upper_feeder_position_state, State(state)); upper_target.get_state_map().emplace(upper_feeder_position_state, State(state)); std::vector state_config_vec = {}; @@ -573,4 +575,4 @@ struct LowerFeederController : public Controller { lower_pidv.sumError = 0.0; lower_feeder_error_monitor = ErrorMonitor{}; } -}; \ No newline at end of file +}; diff --git a/src/controls/controller_manager.cpp b/src/controls/controller_manager.cpp index 3f4a8971c..a20ca658c 100644 --- a/src/controls/controller_manager.cpp +++ b/src/controls/controller_manager.cpp @@ -1,5 +1,6 @@ #include "controller_manager.hpp" #include "comms/data/sendable.hpp" +#include "utils/system_log.hpp" void ControllerManager::init(const std::vector& controller_configurations, CANManager& can, const std::vector& state_config) { available_motors.clear(); @@ -16,7 +17,7 @@ void ControllerManager::init(const std::vector& controller_conf void ControllerManager::init_controller(const Cfg::Controller& controller_config, CANManager& can, const std::vector& state_config) { switch (controller_config.controller_type) { case Cfg::ControllerType::UnsetControllerType: - Serial.println("ControllerManager::init_controller: Unset controller type, skipping"); + SystemLog.error(Subsystem::Controls,"ControllerManager::init_controller: Unset controller type, skipping"); break; case Cfg::ControllerType::XDriveController: controllers.push_back(std::make_unique(controller_config, can, available_motors)); @@ -37,7 +38,7 @@ void ControllerManager::init_controller(const Cfg::Controller& controller_config controllers.push_back(std::make_unique(controller_config, can, available_motors, state_config)); break; default: - Serial.printf("ControllerManager::init_controller: Unrecognized controller type %d\n", controller_config.controller_type); + SystemLog.error(Subsystem::Controls,"ControllerManager::init_controller: Unrecognized controller type %d\n", controller_config.controller_type); break; } } @@ -47,4 +48,4 @@ void ControllerManager::step(RobotStateMap& reference_map, RobotStateMap& estima controller->validate(reference_map, estimate_map); controller->step(reference_map, estimate_map, target_map); } -} \ No newline at end of file +} diff --git a/src/controls/estimator.cpp b/src/controls/estimator.cpp index a9d61c606..9d72e7f4b 100644 --- a/src/controls/estimator.cpp +++ b/src/controls/estimator.cpp @@ -5,6 +5,7 @@ #include "sensors/ICM20649.hpp" #include "utils/vector_math.hpp" #include "utils/wrapping.hpp" +#include "utils/system_log.hpp" #include "sensors/RefSystem.hpp" // Estimator shared checking implementation @@ -275,7 +276,7 @@ void GimbalAndChassisEstimator::step_states(RobotStateMap& updated_state_map, co float overridden_yaw = previous_state_map[yaw_state].get_position(); float error = Utils::wrap(overridden_yaw - yaw_angle, -PI, PI); yaw_angle += error * 0.01; - Serial.printf("Overriding gimbal yaw estimate to %f. Currently %f\n", overridden_yaw, yaw_angle); + SystemLog.info(Subsystem::ESTIMATOR,"Overriding gimbal yaw estimate to %f. Currently %f\n", overridden_yaw, yaw_angle); } while (yaw_angle >= PI) @@ -493,13 +494,13 @@ void LowerFeederEstimator::step_states(RobotStateMap& updated_state_map, const R if (fabs(diff) > 0.5 && fabs(diff) < 2*PI - 0.5 && count > 0) { num_encoder_resets++; reset_value = feeder_angle; - Serial.printf("Feeder angle diff is large: %d, feeder angle: %f, prev feeder angle: %f\n", num_encoder_resets, feeder_angle, prev_feeder_angle); + SystemLog.warn(Subsystem::ESTIMATOR,"Feeder angle diff is large: %d, feeder angle: %f, prev feeder angle: %f\n", num_encoder_resets, feeder_angle, prev_feeder_angle); diff = 0; // set diff to 0 to avoid large jumps in ball count } if (fabs(lower_diff) > 0.5 && fabs(lower_diff) < 2*PI - 0.5 && count > 0) { num_encoder_resets++; reset_value = lower_feeder_angle; - Serial.printf("Lower feeder angle diff is large: %d, lower feeder angle: %f, prev lower feeder angle: %f\n", num_encoder_resets, lower_feeder_angle, prev_lower_feeder_angle); + SystemLog.warn(Subsystem::ESTIMATOR,"Lower feeder angle diff is large: %d, lower feeder angle: %f, prev lower feeder angle: %f\n", num_encoder_resets, lower_feeder_angle, prev_lower_feeder_angle); lower_diff = 0; // set lower_diff to 0 to avoid large jumps in ball count } @@ -518,10 +519,10 @@ void LowerFeederEstimator::step_states(RobotStateMap& updated_state_map, const R lower_ball_count += (lower_diff/(M_PI/lower_feeder_ratio)) * lower_feeder_direction; if (count == 0) { - Serial.printf("Initial feeder ball count: %f, lower feeder ball count: %f\n", ball_count, lower_ball_count); + SystemLog.info(Subsystem::ESTIMATOR,"Initial feeder ball count: %f, lower feeder ball count: %f\n", ball_count, lower_ball_count); while (ball_count - lower_ball_count > 0.5) lower_ball_count += 1.0; while (lower_ball_count - ball_count > 0.5) ball_count += 1.0; - Serial.printf("Adjusted feeder ball count: %f, lower feeder ball count: %f\n", ball_count, lower_ball_count); + SystemLog.info(Subsystem::ESTIMATOR,"Adjusted feeder ball count: %f, lower feeder ball count: %f\n", ball_count, lower_ball_count); count++; } diff --git a/src/sensors/RefSystem.cpp b/src/sensors/RefSystem.cpp index 1c38b6735..ecc2c4a6f 100644 --- a/src/sensors/RefSystem.cpp +++ b/src/sensors/RefSystem.cpp @@ -1,6 +1,7 @@ #include "RefSystem.hpp" #include "RefSystemPacketDefs.hpp" #include "comms/data/sendable.hpp" +#include "utils/system_log.hpp" RefSystem ref; // Global instance uint8_t generateCRC8(const uint8_t *data, uint32_t len) { @@ -88,35 +89,35 @@ uint16_t RefSystem::get_client_id_for_robot(uint16_t robot_id) { bool RefSystem::write_frame(HardwareSerial& serial, uint8_t* packet, uint16_t length) { if (packet == nullptr) { - Serial.println("No Ref packet to send"); + SystemLog.error(Subsystem::REF,"No Ref packet to send"); return false; } if (length > REF_MAX_PACKET_SIZE) { - Serial.println("Packet Too Long to Send!"); + SystemLog.error(Subsystem::REF,"Packet Too Long to Send!"); return false; } if (length < REF_FRAME_OVERHEAD) { - Serial.println("Packet Too Short to Send!"); + SystemLog.error(Subsystem::REF,"Packet Too Short to Send!"); return false; } uint16_t frame_data_length = get_u16(packet + 1); if (frame_data_length > REF_MAX_DATA_SIZE) { - Serial.println("Packet Data Too Long to Send!"); + SystemLog.error(Subsystem::REF,"Packet Data Too Long to Send!"); return false; } uint16_t expected_length = REF_FRAME_OVERHEAD + frame_data_length; if (length != expected_length) { - Serial.println("Packet Length Mismatch!"); + SystemLog.error(Subsystem::REF,"Packet Length Mismatch!"); return false; } uint16_t command_id = get_u16(packet + FrameHeader::packet_size); if (command_id > REF_MAX_COMMAND_ID) { - Serial.println("Invalid Ref command ID"); + SystemLog.error(Subsystem::REF,"Invalid Ref command ID"); return false; } @@ -127,13 +128,13 @@ bool RefSystem::write_frame(HardwareSerial& serial, uint8_t* packet, uint16_t le } if (bytes_sent + length > REF_MAX_BAUD_RATE) { - Serial.println("Too many bytes"); + SystemLog.error(Subsystem::REF,"Too many bytes"); return false; } uint32_t now_us = micros(); if (last_ref_packet_write_us != 0 && now_us - last_ref_packet_write_us < REF_MAX_PACKET_DELAY) { - Serial.println("Ref packet rate limited"); + SystemLog.warn(Subsystem::REF,"Ref packet rate limited"); return false; } @@ -153,24 +154,24 @@ bool RefSystem::write_frame(HardwareSerial& serial, uint8_t* packet, uint16_t le return true; } - Serial.println("Failed to write"); + SystemLog.error(Subsystem::REF,"Failed to write"); return false; } bool RefSystem::write_robot_interaction(uint16_t content_id, const uint8_t* payload, uint16_t payload_length, uint16_t receiver_id) { if (payload_length > RobotInteraction::max_content_size) { - Serial.println("Robot interaction payload too long"); + SystemLog.error(Subsystem::REF,"Robot interaction payload too long"); return false; } if (payload_length > 0 && payload == nullptr) { - Serial.println("No robot interaction payload"); + SystemLog.error(Subsystem::REF,"No robot interaction payload"); return false; } uint16_t sender_id = ref_data.robot_performance.robot_ID; if (sender_id == 0) { - Serial.println("No robot ID for client drawing"); + SystemLog.error(Subsystem::REF,"No robot ID for client drawing"); return false; } @@ -179,7 +180,7 @@ bool RefSystem::write_robot_interaction(uint16_t content_id, const uint8_t* payl } if (receiver_id == 0) { - Serial.println("No player client ID for this robot"); + SystemLog.error(Subsystem::REF,"No player client ID for this robot"); return false; } @@ -233,7 +234,7 @@ bool RefSystem::read_frame_header(HardwareSerial* serial, uint8_t raw_buffer[REF // read and verify header int bytes_read = serial->readBytes(raw_buffer, FrameHeader::packet_size); if (bytes_read != FrameHeader::packet_size) { - Serial.println("Couldnt read enough bytes for Header"); + SystemLog.error(Subsystem::REF,"Couldnt read enough bytes for Header"); packets_failed++; return false; } @@ -241,7 +242,7 @@ bool RefSystem::read_frame_header(HardwareSerial* serial, uint8_t raw_buffer[REF // set read data frame.header.SOF = raw_buffer[buffer_index + 0]; if (frame.header.SOF != 0xA5) { - Serial.println("Not a valid frame"); + SystemLog.error(Subsystem::REF,"Not a valid frame"); return false; } @@ -252,13 +253,13 @@ bool RefSystem::read_frame_header(HardwareSerial* serial, uint8_t raw_buffer[REF // verify the CRC is correct uint8_t expected_CRC = generateCRC8(raw_buffer, 4); if (frame.header.CRC != expected_CRC) { - Serial.printf("[Ref] Header CRC failed: received=0x%02x expected=0x%02x\n", frame.header.CRC, expected_CRC); + SystemLog.error(Subsystem::REF,"[Ref] Header CRC failed: received=0x%02x expected=0x%02x\n", frame.header.CRC, expected_CRC); packets_failed++; return false; } if (frame.header.data_length > REF_MAX_DATA_SIZE) { - Serial.printf("[Ref] Data length too large: %u (max %u)\n", frame.header.data_length, REF_MAX_DATA_SIZE); + SystemLog.error(Subsystem::REF,"[Ref] Data length too large: %u (max %u)\n", frame.header.data_length, REF_MAX_DATA_SIZE); packets_failed++; return false; } @@ -277,7 +278,7 @@ bool RefSystem::read_frame_command_ID(HardwareSerial* serial, uint8_t raw_buffer // read and verify command ID int bytes_read = serial->readBytes(raw_buffer + buffer_index, REF_COMMAND_ID_SIZE); if (bytes_read != REF_COMMAND_ID_SIZE) { - Serial.println("Couldnt read enough bytes for ID"); + SystemLog.error(Subsystem::REF,"Couldnt read enough bytes for ID"); packets_failed++; return false; } @@ -287,7 +288,7 @@ bool RefSystem::read_frame_command_ID(HardwareSerial* serial, uint8_t raw_buffer // sanity check, verify the ID is valid if (frame.commandID > REF_MAX_COMMAND_ID) { - Serial.printf("[Ref] Invalid command ID: 0x%04x\n", frame.commandID); + SystemLog.error(Subsystem::REF,"[Ref] Invalid command ID: 0x%04x\n", frame.commandID); packets_failed++; return false; } @@ -306,7 +307,7 @@ bool RefSystem::read_frame_data(HardwareSerial* serial, uint8_t raw_buffer[REF_M // read and verify data int bytes_read = serial->readBytes(raw_buffer + buffer_index, frame.header.data_length); if (bytes_read != frame.header.data_length) { - Serial.println("Couldnt read enough bytes for Data"); + SystemLog.error(Subsystem::REF,"Couldnt read enough bytes for Data"); packets_failed++; return false; } @@ -328,7 +329,7 @@ int RefSystem::read_frame_tail(HardwareSerial* serial, uint8_t raw_buffer[REF_MA // read and verify tail int bytes_read = serial->readBytes(raw_buffer + buffer_index, REF_FRAME_TAIL_SIZE); if (bytes_read != REF_FRAME_TAIL_SIZE) { - Serial.println("Couldnt read enough bytes for CRC"); + SystemLog.error(Subsystem::REF,"Couldnt read enough bytes for CRC"); packets_failed++; return 0; } @@ -338,7 +339,7 @@ int RefSystem::read_frame_tail(HardwareSerial* serial, uint8_t raw_buffer[REF_MA uint16_t expected_CRC = generateCRC16(raw_buffer, buffer_index); if (frame.CRC != expected_CRC) { - Serial.printf("[Ref] Tail CRC failed: command=0x%04x length=%u received=0x%04x expected=0x%04x\n", + SystemLog.error(Subsystem::REF,"[Ref] Tail CRC failed: command=0x%04x length=%u received=0x%04x expected=0x%04x\n", frame.commandID, frame.header.data_length, frame.CRC, expected_CRC); packets_failed++; return -1; @@ -368,7 +369,7 @@ void RefSystem::set_ref_data(Frame& frame, uint8_t raw_buffer[REF_MAX_PACKET_SIZ FrameType type = static_cast(frame.commandID); #ifdef REF_SYSTEM_DEBUG - Serial.printf("[Ref] command=0x%04x length=%u sequence=%u\n", + SystemLog.info(Subsystem::REF,"[Ref] command=0x%04x length=%u sequence=%u\n", frame.commandID, frame.header.data_length, frame.header.sequence); #endif @@ -460,7 +461,7 @@ void RefSystem::set_ref_data(Frame& frame, uint8_t raw_buffer[REF_MAX_PACKET_SIZ case FrameType::CUSTOM_CLIENT_ROBOT_COMMAND: break; default: - Serial.printf("Ref System::set_ref_data: Unknown Frame Type 0x%04x\n", frame.commandID); + SystemLog.error(Subsystem::REF,"Ref System::set_ref_data: Unknown Frame Type 0x%04x\n", frame.commandID); break; } } @@ -472,9 +473,9 @@ void RefSystem::read_vtm() { if (now_ms - last_vtm_status_print_ms >= 1000) { int available = VTM_SERIAL.available(); if (available > 0) { - Serial.printf("[VTM] waiting: available=%d peek=0x%02x\n", available, VTM_SERIAL.peek()); + SystemLog.info(Subsystem::REF,"[VTM] waiting: available=%d peek=0x%02x\n", available, VTM_SERIAL.peek()); } else { - Serial.printf("[VTM] waiting: available=0\n"); + SystemLog.info(Subsystem::REF,"[VTM] waiting: available=0\n"); } last_vtm_status_print_ms = now_ms; } @@ -491,9 +492,9 @@ void RefSystem::read_vtm() { if (skipped_bytes > 0) { int available = VTM_SERIAL.available(); if (available > 0) { - Serial.printf("[VTM] skipped %u byte(s) before header: available=%d peek=0x%02x\n", skipped_bytes, available, VTM_SERIAL.peek()); + SystemLog.info(Subsystem::REF,"[VTM] skipped %u byte(s) before header: available=%d peek=0x%02x\n", skipped_bytes, available, VTM_SERIAL.peek()); } else { - Serial.printf("[VTM] skipped %u byte(s) before header: available=0\n", skipped_bytes); + SystemLog.error(Subsystem::REF,"[VTM] skipped %u byte(s) before header: available=0\n", skipped_bytes); } } #endif @@ -509,16 +510,16 @@ void RefSystem::read_vtm() { } #ifdef REF_SYSTEM_DEBUG - Serial.printf("[VTM] raw:"); + SystemLog.info(Subsystem::REF,"[VTM] raw:"); for (uint8_t i = 0; i < VTM_REMOTE_CONTROL_PACKET_SIZE; i++) { - Serial.printf(" %02x", packet[i]); + SystemLog.info(Subsystem::REF," %02x", packet[i]); } - Serial.printf("\n"); + SystemLog.info(Subsystem::REF,"\n"); #endif if (packet[1] != VTM_REMOTE_CONTROL_HEADER_2) { #ifdef REF_SYSTEM_DEBUG - Serial.printf("[VTM] bad second header: received=0x%02x expected=0x%02x\n", packet[1], VTM_REMOTE_CONTROL_HEADER_2); + SystemLog.info(Subsystem::REF,"[VTM] bad second header: received=0x%02x expected=0x%02x\n", packet[1], VTM_REMOTE_CONTROL_HEADER_2); #endif packets_failed++; continue; @@ -527,7 +528,7 @@ void RefSystem::read_vtm() { uint16_t received_crc = packet[19] | (static_cast(packet[20]) << 8); uint16_t expected_crc = generateCRC16(packet, VTM_REMOTE_CONTROL_PACKET_SIZE - 2); if (received_crc != expected_crc) { - Serial.printf("[VTM] CRC failed: received=0x%04x expected=0x%04x\n", received_crc, expected_crc); + SystemLog.warn(Subsystem::REF,"[VTM] CRC failed: received=0x%04x expected=0x%04x\n", received_crc, expected_crc); packets_failed++; continue; } @@ -537,7 +538,7 @@ void RefSystem::read_vtm() { #ifdef REF_SYSTEM_DEBUG const VTMRemoteControl &input = ref_data.vtm_remote_control; - Serial.printf("[VTM] mouse=(%d,%d) scroll=%d buttons=%u/%u/%u keys=0x%04x\n", input.mouse_speed_x, input.mouse_speed_y, input.scroll_speed, static_cast(input.button_left), static_cast(input.button_right), static_cast(input.button_middle), input.keyboard_value); + SystemLog.info(Subsystem::REF,"[VTM] mouse=(%d,%d) scroll=%d buttons=%u/%u/%u keys=0x%04x\n", input.mouse_speed_x, input.mouse_speed_y, input.scroll_speed, static_cast(input.button_left), static_cast(input.button_right), static_cast(input.button_middle), input.keyboard_value); #endif return; } diff --git a/src/sensors/buff_encoder.cpp b/src/sensors/buff_encoder.cpp index 25db669ed..b80e57388 100644 --- a/src/sensors/buff_encoder.cpp +++ b/src/sensors/buff_encoder.cpp @@ -1,5 +1,6 @@ #include "buff_encoder.hpp" #include "comms/data/sendable.hpp" +#include "utils/system_log.hpp" const SPISettings BuffEncoder::m_settings = SPISettings(1000000, MT6835_BITORDER, SPI_MODE3); @@ -43,15 +44,15 @@ void BuffEncoder::read() { uint8_t crc_computed = mt6835_crc8(&data[2], 3); if (crc_received != crc_computed) { - Serial.printf("Pin: %u, MT6835 CRC mismatch\n", config_data.spi_cs); + SystemLog.error(Subsystem::SENSORS,"Pin: %u, MT6835 CRC mismatch\n", config_data.spi_cs); return; } if (status & MT6835_STATUS_UNDERVOLT) { - Serial.printf("Pin: %u, MT6835 undervoltage detected\n", config_data.spi_cs); + SystemLog.error(Subsystem::SENSORS,"Pin: %u, MT6835 undervoltage detected\n", config_data.spi_cs); return; } if (status & MT6835_STATUS_WEAKFIELD) { - Serial.printf("Pin: %u, MT6835 weak field detected\n", config_data.spi_cs); + SystemLog.error(Subsystem::SENSORS,"Pin: %u, MT6835 weak field detected\n", config_data.spi_cs); return; } @@ -71,7 +72,7 @@ void BuffEncoder::read() { void BuffEncoder::write_zero_pos(uint16_t zero_pos_raw) { if (zero_pos_raw > 0x0FFF) { - Serial.printf("Pin: %u, ZERO_POS value out of range: %u\n", config_data.spi_cs, zero_pos_raw); + SystemLog.error(Subsystem::SENSORS,"Pin: %u, ZERO_POS value out of range: %u\n", config_data.spi_cs, zero_pos_raw); return; } @@ -121,7 +122,7 @@ void BuffEncoder::write_zero_pos(uint16_t zero_pos_raw) { digitalWrite(config_data.spi_cs, HIGH); SPI.endTransaction(); - Serial.printf("Pin: %u, wrote ZERO_POS = 0x%03X (%u)\n", + SystemLog.info(Subsystem::SENSORS,"Pin: %u, wrote ZERO_POS = 0x%03X (%u)\n", config_data.spi_cs, zero_pos_raw, zero_pos_raw); } @@ -160,7 +161,7 @@ float BuffEncoder::read_zero_pos() { uint16_t zero_pos_raw = (static_cast(zero_pos_high) << 4) | zero_pos_low; // 12-bit value, 0-4095 if (zero_pos_raw != 0) { - Serial.printf("Pin: %u, ZERO_POS raw = 0x%03X (%u), degrees = %.3f\n", + SystemLog.info(Subsystem::SENSORS,"Pin: %u, ZERO_POS raw = 0x%03X (%u), degrees = %.3f\n", config_data.spi_cs, zero_pos_raw, zero_pos_raw, zero_pos_raw * (360.0f / 4096.0f)); } diff --git a/src/sensors/can/GIM.cpp b/src/sensors/can/GIM.cpp index 1f6571308..2ba9ab46c 100644 --- a/src/sensors/can/GIM.cpp +++ b/src/sensors/can/GIM.cpp @@ -1,4 +1,5 @@ #include "GIM.hpp" +#include "utils/system_log.hpp" void GIM::init() { write_motor_on(); @@ -42,7 +43,7 @@ int GIM::read(CAN_message_t& msg) { break; } default: - Serial.printf("No GIM::read case for this command byte: 0x%02X\n", cmd_byte); + SystemLog.error(Subsystem::MOTORS,"No GIM::read case for this command byte: 0x%02X\n", cmd_byte); break; } diff --git a/src/sensors/can/MG8016EI6.cpp b/src/sensors/can/MG8016EI6.cpp index 12ec968c0..9650a26eb 100644 --- a/src/sensors/can/MG8016EI6.cpp +++ b/src/sensors/can/MG8016EI6.cpp @@ -1,4 +1,5 @@ #include "MG8016EI6.hpp" +#include "utils/system_log.hpp" void MG8016EI6::init() { write_motor_on(); @@ -124,7 +125,7 @@ int MG8016EI6::read(CAN_message_t& msg) { break; } default: - Serial.printf("Unknown command byte: 0x%02X\n", cmd_byte); + SystemLog.error(Subsystem::MOTORS,"Unknown command byte: 0x%02X\n", cmd_byte); break; } @@ -537,4 +538,4 @@ void MG8016EI6::create_cmd_read_state_3(uint8_t buf[8]) { buf[5] = 0; buf[6] = 0; buf[7] = 0; -} \ No newline at end of file +} diff --git a/src/sensors/can/SDC104.cpp b/src/sensors/can/SDC104.cpp index e9c1f6efc..740d7e3ba 100644 --- a/src/sensors/can/SDC104.cpp +++ b/src/sensors/can/SDC104.cpp @@ -1,4 +1,6 @@ #include "SDC104.hpp" +#include "utils/system_log.hpp" + void SDC104::init() { // axis state is defered until the first control command is sent @@ -38,13 +40,13 @@ int SDC104::read(CAN_message_t& msg) { break; } default: { - Serial.printf("Unknown error type: %d\n", static_cast(m_error_request_type)); + SystemLog.error(Subsystem::MOTORS,"Unknown error type: %d\n", static_cast(m_error_request_type)); break; } } break; } - case CMD_MIT_CONTROL: { Serial.printf("MIT control command not implemented\n"); break; } + case CMD_MIT_CONTROL: { SystemLog.error(Subsystem::MOTORS,"MIT control command not implemented\n"); break; } case CMD_GET_ENCODER_ESTIMATES: { m_position_estimate = *((float*)&msg.buf[0]); m_velocity_estimate = *((float*)&msg.buf[4]); @@ -91,7 +93,7 @@ int SDC104::read(CAN_message_t& msg) { break; } default: { - Serial.printf("Unknown command byte: %d\n", cmd_byte); + SystemLog.error(Subsystem::MOTORS,"Unknown command byte: %d\n", cmd_byte); break; } } diff --git a/src/sensors/can/can_manager.cpp b/src/sensors/can/can_manager.cpp index 30d6db0e7..970154454 100644 --- a/src/sensors/can/can_manager.cpp +++ b/src/sensors/can/can_manager.cpp @@ -2,6 +2,7 @@ // driver includes are here not in header since they're only needed in the implementation #include "utils/safety.hpp" +#include "utils/system_log.hpp" #include "sensors/can/C610.hpp" #include "sensors/can/C620.hpp" #include "sensors/can/MG8016EI6.hpp" @@ -97,7 +98,7 @@ void CANManager::read() { // how would this happen? if (distribute_msg(msg) == Cfg::MotorName::UnsetMotorName) { // - 1 on msg.bus to maintain bus IDs being 0-indexed - Serial.printf("CANManager failed to distribute message with raw CAN ID: %.4x on bus: %x\n", msg.id, msg.bus - 1); + SystemLog.error(Subsystem::CAN,"CANManager failed to distribute message with raw CAN ID: %.4x on bus: %x\n", msg.id, msg.bus - 1); } } } diff --git a/src/sensors/transmitter/ET16S.cpp b/src/sensors/transmitter/ET16S.cpp index 5cc27176b..8c283efeb 100644 --- a/src/sensors/transmitter/ET16S.cpp +++ b/src/sensors/transmitter/ET16S.cpp @@ -2,6 +2,7 @@ #include "sensors/RefSystem.hpp" #include "comms/data/sendable.hpp" #include "comms/config_data/state.hpp" +#include "utils/system_log.hpp" ET16S::ET16S(const Cfg::ET16S& config) : config(config) { } @@ -78,7 +79,7 @@ void ET16S::read() { void ET16S::print() { for (int i = 0; i < ET16S_INPUT_VALUE_COUNT; i++) { - Serial.printf("%f ", channel[i].data); + SystemLog.info(Subsystem::SENSORS,"%f ", channel[i].data); } Serial.println(); @@ -86,7 +87,7 @@ void ET16S::print() { void ET16S::print_raw() { for (int i = 0; i < ET16S_INPUT_VALUE_COUNT; i++) { - Serial.printf("%.3u ", channel[i].raw_format); + SystemLog.info(Subsystem::SENSORS,"%.3u ", channel[i].raw_format); } Serial.println(); diff --git a/src/sensors/transmitter/dr16.cpp b/src/sensors/transmitter/dr16.cpp index 4e8ef1915..e825965c2 100644 --- a/src/sensors/transmitter/dr16.cpp +++ b/src/sensors/transmitter/dr16.cpp @@ -2,6 +2,7 @@ #include #include "sensors/RefSystem.hpp" #include "comms/data/sendable.hpp" +#include "utils/system_log.hpp" DR16::DR16(const Cfg::DR16& config_) : config(config_) { @@ -63,7 +64,7 @@ void DR16::read() { while (last_available == Serial8.available() && micros() - start < DR16_ALIGNMENT_LONG_INTERVAL_THRESHOLD); uint32_t end = micros(); - Serial.printf("DR16: Still aligning (%d)\n", interval_count); + SystemLog.warn(Subsystem::SENSORS,"DR16: Still aligning (%d)\n", interval_count); // if this interval was a long interval (break in packets), call the alignment done and finish up // also mark this as a successful alignment, rather than it timing out @@ -75,11 +76,11 @@ void DR16::read() { // print success or failure if (alignment_timed_out) { - Serial.printf("DR16: Alignment timed out, trying again next loop\n\n"); + SystemLog.error(Subsystem::SENSORS,"DR16: Alignment timed out, trying again next loop\n\n"); } else { uint32_t align_end = micros(); - Serial.printf("DR16: Aligned successfully\n"); - Serial.printf("DR16: Alignment took %fms\n\n", (align_end - align_start) / 1000.f); + SystemLog.error(Subsystem::SENSORS,"DR16: Aligned successfully\n"); + SystemLog.error(Subsystem::SENSORS,"DR16: Alignment took %fms\n\n", (align_end - align_start) / 1000.f); } // clear the buffer to get ready for the next packet diff --git a/src/utils/system_log.cpp b/src/utils/system_log.cpp index 86998a74d..3c8c305dc 100644 --- a/src/utils/system_log.cpp +++ b/src/utils/system_log.cpp @@ -10,7 +10,8 @@ const char* sys_to_str(Subsystem sys) { case Subsystem::SENSORS: return "SEN"; case Subsystem::ESTIMATOR: return "EST"; case Subsystem::COMMS: return "COM"; - case Subsystem::REF: return "REF"; + case Subsystem::REF: return "REF"; + case Subsystem::Controls: return "Con"; default: return "GEN"; } } diff --git a/src/utils/system_log.hpp b/src/utils/system_log.hpp index 1cb13fb44..7f20c73f7 100644 --- a/src/utils/system_log.hpp +++ b/src/utils/system_log.hpp @@ -3,7 +3,7 @@ enum class LogLevel { INFO, WARN, ERROR }; /// @brief List of robot subsystems -enum class Subsystem { ALL, GENERAL, CAN, MOTORS, SENSORS, ESTIMATOR, COMMS , REF}; +enum class Subsystem { ALL, GENERAL, CAN, MOTORS, SENSORS, ESTIMATOR, COMMS , REF, Controls}; /// @brief Event log struct which holds all relevent data about potential print struct LogEvent { /// @brief current loop count From 2ebbf52282dc92f7770a2b236429531b7d2fb6e1 Mon Sep 17 00:00:00 2001 From: EricV Date: Tue, 1 Sep 2026 20:47:43 -0600 Subject: [PATCH 40/41] cam trigger messages --- src/sensors/StereoCamTrigger.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/sensors/StereoCamTrigger.cpp b/src/sensors/StereoCamTrigger.cpp index adbddaf2e..629c3aead 100644 --- a/src/sensors/StereoCamTrigger.cpp +++ b/src/sensors/StereoCamTrigger.cpp @@ -1,8 +1,10 @@ #include "StereoCamTrigger.hpp" #include "comms/data/sendable.hpp" #include "sensors/transmitter/transmitter_utils.hpp" +#include "utils/system_log.hpp" #include + std::unique_ptr* StereoCamTrigger::estimated_state_map_interrupt_safe = nullptr; StereoCamTrigger::StereoCamTrigger(const Cfg::StereoCamTrigger& config): Sensor(), config(config), comms_data(config.camera_trigger_name) {} @@ -76,7 +78,7 @@ void StereoCamTrigger::read() { counter = -1; - Serial.printf("counter reset pin: %u triggered\n", config.camera_1_line_2_pin); + SystemLog.info(Subsystem::SENSORS,"counter reset pin: %u triggered\n", config.camera_1_line_2_pin); } Comms::comms_layer.get_hive_data().stereo_cam_start_stop.stop_received = false; Comms::comms_layer.get_hive_data().stereo_cam_start_stop.start_received = false; From d809a6b40c2fff192ae07a941d1b0bdc9df00359 Mon Sep 17 00:00:00 2001 From: EricV Date: Sat, 5 Sep 2026 18:21:54 -0600 Subject: [PATCH 41/41] adjusted to PR comments --- src/hello_robot.cpp | 5 +++-- src/main.cpp | 1 - src/utils/profiler.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index 87ed3a27b..bfe7e9ce7 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -233,7 +233,7 @@ void HelloRobot::check_safety() { if (not_safety_mode) { // SAFETY OFF can.write(); - SystemLog.info(Subsystem::CAN,"Can write\n"); + //SystemLog.info(Subsystem::CAN,"Can write\n"); } else { // SAFETY ON // TODO: Reset all controller integrators here @@ -482,8 +482,9 @@ void HelloRobot::cmd_live() { if (num_active_views > 0) { last_redraw_time = 0; - Serial.print("\033[2J"); + Serial.print("\033[2J"); } else { + SystemLog.is_live_view_active = false; Serial.println("Usage: live [prof] [tx] [sensors] [estimated_state] [target_state] [heartbeat]"); } } diff --git a/src/main.cpp b/src/main.cpp index 62215b1bf..c41ec3ec0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,7 +5,6 @@ int main() { Serial.begin(115200); // the serial monitor is actually always active (for // debug use Serial.println & tycmd) - while(!Serial); debug.begin(SerialUSB1); Utils::print_logo(); diff --git a/src/utils/profiler.hpp b/src/utils/profiler.hpp index c04ec8c30..3864139f8 100644 --- a/src/utils/profiler.hpp +++ b/src/utils/profiler.hpp @@ -21,7 +21,7 @@ struct Profiler { /// @brief max time of section uint32_t max_time = 0; /// @brief Number of start/end times recorded - uint16_t count = 0; + uint32_t count = 0; /// @brief Label on if a begin() has been called and the corresponding end() hasn't yet uint8_t started = 0; /// @brief Name for each section