Command Line Interface

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

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<ConfigurationStatusData> 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);
     }
 }

src/comms/ethernet_comms.cpp

@@ -1,36 +1,37 @@
 #include "ethernet_comms.hpp"
+#include "utils/system_log.hpp"
 
 namespace Comms {
 
 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
 	}
 
 	// begin the UDP server
 	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,16 +58,16 @@ 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;
 		}
 
 	}
 
 	// 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
+}	// namespace Comms

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);
-}
+}

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<Cfg::State> state_config_vec = {};
@@ -573,4 +575,4 @@ struct LowerFeederController : public Controller {
             lower_pidv.sumError = 0.0;
             lower_feeder_error_monitor = ErrorMonitor{};
         }
-};
+};

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<Cfg::Controller>& controller_configurations, CANManager& can, const std::vector<Cfg::State>& state_config) {
     available_motors.clear();
@@ -16,7 +17,7 @@ void ControllerManager::init(const std::vector<Cfg::Controller>& controller_conf
 void ControllerManager::init_controller(const Cfg::Controller& controller_config, CANManager& can, const std::vector<Cfg::State>& 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<XDriveController>(controller_config, can, available_motors));
@@ -37,7 +38,7 @@ void ControllerManager::init_controller(const Cfg::Controller& controller_config
             controllers.push_back(std::make_unique<LowerFeederController>(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);
     }
-}
+}

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++;
     }