diff --git a/Makefile b/Makefile index fabddd32..605d8537 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 +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,9 +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 - TARGET := firmware TARGET_ELF := $(BUILD_DIR)/$(TARGET).elf TARGET_HEX := $(BUILD_DIR)/$(TARGET).hex @@ -101,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) @@ -152,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 @@ -179,7 +187,8 @@ upload: build @echo [Uploading] - If this fails, press the button on the teensy and re-run 'make upload' @tycmd upload $(TARGET_HEX) @sleep 0.4s - @bash $(TOOLS_DIR)/monitor.sh +#@bash $(TOOLS_DIR)/monitor.sh + @tycmd monitor # Install requirements for building and uploading firmware @@ -199,7 +208,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 diff --git a/src/comms/comms_layer.cpp b/src/comms/comms_layer.cpp index 2ed3a636..b02e4032 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 61ea3cb9..29534d92 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 f8c1d283..a36f0577 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 b9392815..63044e76 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 3f4a8971..a20ca658 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 a9d61c60..9d72e7f4 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/controls/robot_state_map.cpp b/src/controls/robot_state_map.cpp index 5e8d260f..e7faf6f0 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); + } +} diff --git a/src/hello_robot.cpp b/src/hello_robot.cpp index b068db31..bfe7e9ce 100644 --- a/src/hello_robot.cpp +++ b/src/hello_robot.cpp @@ -1,7 +1,6 @@ #include "hello_robot.hpp" - #ifdef PROFILER -Profiler prof; +Profiler prof; #endif void HelloRobot::init() { @@ -62,23 +61,48 @@ void HelloRobot::run() { // 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(); - loop_timing(); - } + 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(); + } } -void HelloRobot::crash_report() { - // over Serial in the future, we'll send this directly over comms - if (CrashReport) { - while (1) { - Serial.println(CrashReport); - Serial.println("\nReflash to clear CrashReport (and also please " - "fix why it crashed)"); - delay(1000); - } - } + +void HelloRobot::crash_report(){ + // over Serial in the future, we'll send this directly over comms + if (CrashReport) { + while (1) { + Serial.println(CrashReport); + Serial.println("\nReflash to clear CrashReport (and also please " + "fix why it crashed)"); + delay(1000); + } + } } void HelloRobot::read_telemetry() { // read CAN and send motor states to comms @@ -98,10 +122,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 @@ -119,7 +139,8 @@ void HelloRobot::process_behaviors() { // clear the request Comms::comms_layer.get_hive_data().override_state_data.active = false; - hive_state_map_offset->from_comms_packet(Comms::comms_layer.get_hive_data().override_state_data.state); + 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; override_request = true; @@ -128,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; @@ -138,7 +158,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())) { - Serial.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); } @@ -184,20 +204,20 @@ 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); - // 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"); - reset_teensy(); - } - } else { - slow_loop_counter = 0; - } - } - last_loop_slow = is_slow_loop; + 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.error("Kowabunga bitches\n"); + reset_teensy(); + } + } else { + slow_loop_counter = 0; + } + } + last_loop_slow = is_slow_loop; if (!last_gimbal_power && ref.ref_data.robot_performance.gimbal_power_active) { gimbal_power_timer.start(); @@ -213,7 +233,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 @@ -231,6 +251,10 @@ 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); + } // LED heartbeat -- linked to loop count to reveal slowdowns and // freezes. loopc % (int)(1E3 / float(HEARTBEAT_FREQ)) < (int)(1E3 / float(5 * HEARTBEAT_FREQ)) ? digitalWrite(13, HIGH) : digitalWrite(13, LOW); @@ -242,3 +266,303 @@ 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() { + // ========================================== + // 2. LIVE VIEW RENDERER + // ========================================== + if (num_active_views > 0) { + + 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 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::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 + } + SystemLog.draw_dashboard_box(); // puts all non-CLI prints in neat box + Serial.println("\n[ LIVE MODE ACTIVE - PRESS ENTER TO EXIT ]"); + + // \033[J clears everything *below* the cursor. + Serial.print("\033[J"); + + last_redraw_time = millis(); + } + + // 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; + } + + return; + } + + // ========================================== + // 3. NORMAL CLI PARSING + // ========================================== + /* Adding new commands is simple. + add command to command dictionary block below + If it is live add to live mode enum and switch statement above + as well as to view_dict lookup table in cmd_live function + */ + while (Serial.available() > 0) { + char c = Serial.read(); + + if (c == '\n' || c == '\r') { + if (cli_index == 0) continue; + + cli_buffer[cli_index] = '\0'; + + // --- 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}, + {"log", &HelloRobot::cmd_log} + }; + + // --- 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"); + } + } + + 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(); + 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 ENTER 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(" 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."); +} +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; + + // 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 + } + } + } + + if (num_active_views > 0) { + last_redraw_time = 0; + Serial.print("\033[2J"); + } else { + SystemLog.is_live_view_active = false; + 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, " "); + + // --- 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; + + // Check Subsystem (if provided) + if (sys_tok) { + 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; + } + } + + // Check Priority Level (if provided) + 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; + } + } + + // 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"); + Serial.println(" Priorities: info, warn, error"); + Serial.println(" Example: log motors warn"); + return; + } + + 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 d594e0ee..fadb06d3 100644 --- a/src/hello_robot.hpp +++ b/src/hello_robot.hpp @@ -13,7 +13,6 @@ #include "sensors/buff_encoder.hpp" #include "comms/config_data/state.hpp" #include "utils/boot_splash.hpp" -#include "utils/profiler.hpp" #include "sensors/d200.hpp" #include "sensors/transmitter/transmitter_manager.hpp" @@ -22,11 +21,13 @@ #include "controls/controller_manager.hpp" #include "controls/estimator_manager.hpp" #include "sensors/RefSystem.hpp" + #include "sensors/StereoCamTrigger.hpp" -#include "utils/profiler.hpp" #include "sensors/sensor_manager.hpp" #include +#include "utils/profiler.hpp" +#include "utils/system_log.hpp" #include #include "comms/data/hive_data.hpp" @@ -41,6 +42,9 @@ extern "C" void reset_teensy(void); #define LOOP_FREQ 1000 #define HEARTBEAT_FREQ 2 +#ifdef PROFILER +extern Profiler prof; +#endif /// @brief Coordinates all hardware, networking, and control systems. class HelloRobot { @@ -139,26 +143,62 @@ class HelloRobot { /// @brief Hive offset state std::optional hive_state_map_offset; - - /// @brief check to see if there is a crash report, and if so, print it repeatedly - void crash_report(); - - /// @brief Reads data from CAN, RefSystem, Transmitter, and Sensors. + // ========================================== + // CLI Variables + // ========================================== + /// @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; + /// @brief array of current live views + LiveMode active_views[MAX_LIVE_VIEWS]; + /// @brief number of active live views + uint8_t num_active_views = 0; + /// @brief 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}; + /// @brief index for cli_buffer + uint8_t cli_index = 0; + /// @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(); + /// @brief CLI function to handle logging + void cmd_log(); + // ========================================== + // Major Loop functions + // ========================================== + /// @brief check to see if there is a crash report, and if so, print it repeatedly + void crash_report(); + + /// @brief Reads data from CAN, RefSystem, Transmitter, and Sensors. void read_telemetry(); - - /// @brief Processes manual inputs, hive modes, and state overrides. - void process_behaviors(); - - /// @brief Steps estimators, governors, and controllers to generate motor targets. + + /// @brief Processes manual inputs, hive modes, and state overrides. + void process_behaviors(); + + /// @brief Steps estimators, governors, and controllers to generate motor targets. void update_controls(); - - /// @brief Checks loop timing/safety constraints and writes to the CAN bus. + + /// @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 LED hearbeat, feeds the watchdog, and ensures consistent loop time. - void loop_timing(); - public: +public: /** * @brief Bootstraps the robot's architecture. * * Downloads the active configuration from the Hive data layer and uses it diff --git a/src/sensors/AdafruitIMUSensor.cpp b/src/sensors/AdafruitIMUSensor.cpp index e552a539..3a0dcd6b 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 SENSOR 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 ef45bb41..e58120eb 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/RefSystem.cpp b/src/sensors/RefSystem.cpp index 1c38b673..ecc2c4a6 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/StereoCamTrigger.cpp b/src/sensors/StereoCamTrigger.cpp index 6cb69807..629c3aea 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; @@ -90,3 +92,8 @@ void StereoCamTrigger::send_to_comms() const { interrupts(); sendable.send_to_comms(); } + +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 8275ef5e..f09bd84b 100644 --- a/src/sensors/StereoCamTrigger.hpp +++ b/src/sensors/StereoCamTrigger.hpp @@ -58,6 +58,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 diff --git a/src/sensors/buff_encoder.cpp b/src/sensors/buff_encoder.cpp index 79e0526b..b80e5738 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)); } @@ -178,6 +179,11 @@ void BuffEncoder::print() const{ Serial.printf("Buff Encoder:\n\t"); Serial.println(get_angle()); } +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()); +} uint8_t BuffEncoder::mt6835_crc8(const uint8_t* data, size_t len) const { constexpr uint8_t poly = 0x07; // X^8 + X^2 + X + 1 diff --git a/src/sensors/buff_encoder.hpp b/src/sensors/buff_encoder.hpp index d7771efd..be260fdd 100644 --- a/src/sensors/buff_encoder.hpp +++ b/src/sensors/buff_encoder.hpp @@ -77,6 +77,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; /// @brief Compute CRC8 per MT6835 datasheet spec (poly = X^8 + X^2 + X + 1, MSB first) /// @param data Pointer to the 3 bytes covering ANGLE[20:0] + STATUS[2:0] (i.e. data[2], data[3], data[4] from the angle burst read) diff --git a/src/sensors/can/GIM.cpp b/src/sensors/can/GIM.cpp index 1f657130..2ba9ab46 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 12ec968c..9650a26e 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 e9c1f6ef..740d7e3b 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 30d6db0e..97015445 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/d200.cpp b/src/sensors/d200.cpp index 99aa9598..3f49a42a 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 f46bd99f..5ef2d719 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 ab543629..879979e5 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 213a6ba5..6a494ab2 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 53b939a3..e4f5d534 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 28a8a988..b83de4ce 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 f31845dc..27553b69 100644 --- a/src/sensors/sensor.hpp +++ b/src/sensors/sensor.hpp @@ -19,8 +19,10 @@ virtual void read() = 0; /// @brief Bind local state map with estimated state map /// @param map is the global estimated state map virtual void provide_isr_map(std::unique_ptr *map) {} - + /// @brief Send the current sensor data to the comms layer. virtual void send_to_comms() const = 0; +/// @brief Prints a formatted dashboard of live sensor values. Default does nothing. +virtual void print_live_data(){} }; diff --git a/src/sensors/sensor_manager.cpp b/src/sensors/sensor_manager.cpp index 56f35f6f..5d7f3f3f 100644 --- a/src/sensors/sensor_manager.cpp +++ b/src/sensors/sensor_manager.cpp @@ -78,3 +78,10 @@ void SensorManager::send_to_comms() { sensor->send_to_comms(); } } + +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 0ed8ec7c..c122fb31 100644 --- a/src/sensors/sensor_manager.hpp +++ b/src/sensors/sensor_manager.hpp @@ -37,7 +37,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 diff --git a/src/sensors/transmitter/ET16S.cpp b/src/sensors/transmitter/ET16S.cpp index 7ea840e1..8c283efe 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(); @@ -107,7 +108,43 @@ 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"); + + 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/ET16S.hpp b/src/sensors/transmitter/ET16S.hpp index ec82797d..9c42a485 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 d62610a5..e825965c 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 @@ -427,3 +428,44 @@ 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"); + + 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()); + + // 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()); + 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 bcde9ab5..e8dea1f3 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 36c667a5..fa22adfc 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 e83787bc..6284a5b5 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 0031705b..9fd16575 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 1195c8b3..28ca86a1 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,56 +27,50 @@ 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 (!sections[i].started) return; + + uint32_t delta = micros() - sections[i].start_time; - // 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; + // 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; } } + + sections[i].count++; + sections[i].started = 0; return; } } #endif } -void Profiler::print(const char *name) { +void Profiler::print_summary() { #ifdef PROFILER - uint32_t sum = 0; - uint32_t min = UINT32_MAX; - uint32_t max = 0; + Serial.println("\n================ PROFILER SUMMARY ================"); + Serial.println(" Subsystem | Avg Time (us) | Max Time (us) "); + Serial.println("--------------------------------------------------"); - // 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 = sections[i].time_lengths[j]; - sum += delta; - if (delta < min) - min = delta; - if (delta > max) - max = 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)); - return; + if (sections[i].name[0] != '\0' && sections[i].count > 0) { + + Serial.printf(" %-16s | %13u | %13u \n", + sections[i].name, + (uint32_t)sections[i].avg_time, + sections[i].max_time); } } + Serial.println("==================================================\n"); #endif } diff --git a/src/utils/profiler.hpp b/src/utils/profiler.hpp index a0d6a3c7..3864139f 100644 --- a/src/utils/profiler.hpp +++ b/src/utils/profiler.hpp @@ -3,7 +3,7 @@ #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 @@ -14,14 +14,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; + 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 @@ -41,7 +41,9 @@ 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(); }; extern Profiler prof; // Global profiler diff --git a/src/utils/system_log.cpp b/src/utils/system_log.cpp new file mode 100644 index 00000000..3c8c305d --- /dev/null +++ b/src/utils/system_log.cpp @@ -0,0 +1,169 @@ +#include "system_log.hpp" + +// 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"; + case Subsystem::Controls: return "Con"; + 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; + + // 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) { + for (size_t i = 0; i < size; i++) { write(buffer[i]); } + return size; +} + +void SystemLogger::push_message() { + current_line[line_length] = '\0'; + + // 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); + + // 2. If CLI is closed, print immediately with colors! + 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), + sys_to_str(messages[head].sys), + messages[head].text); + } + + // 3. Advance circular buffer + head = (head + 1) % LOG_HISTORY; + if (count < LOG_HISTORY) count++; + + line_length = 0; + current_level = LogLevel::INFO; // Reset to default + current_sys = Subsystem::GENERAL; // Reset to default +} +void SystemLogger::log_format(LogLevel lvl, Subsystem sys, const char* format, va_list args) { + char temp[MAX_LINE_LEN]; + vsnprintf(temp, MAX_LINE_LEN, format, 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(lvl, sys); + strncpy(current_line, temp, MAX_LINE_LEN); + line_length = len; + push_message(); +} + +// --- Info Overloads --- +void SystemLogger::info(Subsystem sys, const char* format, ...) { + va_list args; + va_start(args, format); + log_format(LogLevel::INFO, sys, format, args); + va_end(args); +} +void SystemLogger::info(const char* format, ...) { + va_list args; + va_start(args, format); + log_format(LogLevel::INFO, Subsystem::GENERAL, format, args); + va_end(args); +} + +// --- 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, ...) { + va_list args; + va_start(args, format); + 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); +} + +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) { + Serial.println(" No recent events."); + } else { + uint8_t start = (count == LOG_HISTORY) ? head : 0; + for (uint8_t i = 0; i < count; i++) { + uint8_t idx = (start + i) % LOG_HISTORY; + LogEvent& ev = messages[idx]; + + if (should_show(ev.level, ev.sys)) { + 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 new file mode 100644 index 00000000..7f20c73f --- /dev/null +++ b/src/utils/system_log.hpp @@ -0,0 +1,109 @@ +#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, Controls}; +/// @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; + /// @brief contains message with max length 80 characters + char text[80]; +}; + +/// @brief Serial wrapper for handling print statements +class SystemLogger : public Print { +private: + /// @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; + /// @brief max length including the timestamp + static const int MAX_STORED_LEN = 100; + /// @brief message circuluar buffer + LogEvent messages[LOG_HISTORY]; + /// @brief start of the message + uint8_t head = 0; + /// @brief number of characters in current message + uint8_t count = 0; + + /// @brief A temporary buffer to hold the line while print() is building it + char current_line[MAX_LINE_LEN] = {0}; + /// @brief length of print line in buffer + uint8_t line_length = 0; + + /// @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 + /// @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 + /// @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; + /// @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; + + + /// @brief implements print class print for general print statements. + /// @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 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, ...); + /// @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(); +}; + +// Declare a global instance so you can use it everywhere, just like 'Serial' +extern SystemLogger SystemLog;