Feature unit testing

.gitignore

@@ -28,7 +28,6 @@ compile_commands.json
 # Ignore any tools/ directory artifacts
 tools/*.txt
 tools/*.out
-tools/git_scraper
 tools/custom_monitor
 tools/compiler
 tools/tools

README.md

@@ -44,6 +44,11 @@ There are a few other nice helper functions within the makefile. This will list
 make help
 ```
 
+To run tests:
+```bash
+make test
+```
+
 
 ## Contributing
 `main` is the production branch, which is required to be in an always working state.

src/comms/config_data/controller.hpp

@@ -5,7 +5,7 @@
 #include "comms/data/comms_data.hpp" // for CommsData, TypeLabel, to_string
 #include "comms/config_data/motor.hpp" // for Motor
 #include "state.hpp" // for StateName
-#include "safety.hpp" // for assert_or_safety_mode
+#include "utils/safety.hpp" // for assert_or_safety_mode
 
 /// These values are arbitrary limits for the size of the arrays in the Controller struct. 
 // They should be large enough to accommodate any reasonable number of motors, states, or sub controllers per controller configuration.

src/comms/config_data/estimator.hpp

@@ -5,7 +5,7 @@
 #include "state.hpp"      // for StateName
 #include "motor.hpp"      // for MotorName
 #include "sensor.hpp"     // for SensorName
-#include "safety.hpp" // for assert_or_safety_mode
+#include "utils/safety.hpp" // for assert_or_safety_mode
 
 /// These values are arbitrary limits for the size of the arrays in the Estimator struct. 
 // They should be large enough to accommodate any reasonable number of states, motors, or sensors used by an estimator configuration.

src/comms/data/hive_data.cpp

@@ -1,6 +1,6 @@
 #include "hive_data.hpp"
 #include "comms_data.hpp"
-#include "config_data/sensor.hpp"
+#include "comms/config_data/sensor.hpp"
 
 #include "comms/comms_layer.hpp"            // for CommsLayer
 

src/comms/data/packet_payload.cpp

@@ -1,5 +1,5 @@
 #include "packet_payload.hpp"
-#include "safety.hpp"
+#include "utils/safety.hpp"
 
 #include <algorithm>                        // for min
 #include "comms/comms_layer.hpp"            // for CommsLayer

src/comms/data/sendable.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <type_traits>                          // for is_base_of, is_copy_constructible
-#include "comms_layer.hpp"                      // for CommsLayer
+#include "comms/comms_layer.hpp"                // for CommsLayer
 
 namespace Comms {
 

src/comms/ethernet_comms.hpp

@@ -8,10 +8,6 @@ namespace qn = qindesign::network;
 #include "utils/timing.hpp"				// for Timer
 #include "comms/ethernet_packet.hpp"	// for EthernetPacket
 
-// DEBUG define for displaying all comms errors/status updates
-// This is very noisy on start up
-// #define COMMS_DEBUG
-
 namespace Comms {
 
 /// @brief Ethernet Communications. This handles all comms between the Jetson and the Teensy via Ethernet
@@ -105,4 +101,4 @@ class EthernetComms {
 	Timer m_regulation_timer;
 };
 
-}	// namespace Comms
+}	// namespace Comms

src/controls/controller.cpp

@@ -1,6 +1,7 @@
 #include "controller.hpp"
-#include "motor.hpp"
+#include "sensors/can/motor.hpp"
 #include "sensors/RefSystem.hpp"
+#include <cmath>
 
 namespace {
 /// @brief Unwrap a potentially wrapped error value to maintain continuity across wrap boundaries.
@@ -179,22 +180,21 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat
         outputp[2] = pidp[2].filter(dt, false, false);
         outputv[2] = pidv[2].filter(dt, false, false);
         output[2] = (outputp[2] + outputv[2]) * controller_config.gear_ratios.chassis_rad_to_motor_rad;
-        float chassis_heading = estimate_map[Cfg::StateName::ChassisHeading].get_position();
 
-        // Convert to motor velocities
-        motor_velocity[1] = output[0] * cos(chassis_heading) + output[1] * sin(chassis_heading) + output[2];
-        motor_velocity[2] = output[0] * sin(chassis_heading) - output[1] * cos(chassis_heading) + output[2];
-        motor_velocity[3] = -output[0] * cos(chassis_heading) - output[1] * sin(chassis_heading) + output[2];
-        motor_velocity[0] = -output[0] * sin(chassis_heading) + output[1] * cos(chassis_heading) + output[2];
+        float chassis_heading = estimate_map[Cfg::StateName::ChassisHeading].get_position();
+        MotorVelocities mv = xdrive_mix(output[0], output[1], output[2], chassis_heading);
+        // position mode uses [1,2,3,0] index order
+        motor_velocity[1] = mv.v[0];
+        motor_velocity[2] = mv.v[1];
+        motor_velocity[3] = mv.v[2];
+        motor_velocity[0] = mv.v[3];
 
         // Power limiting
-        float power_buffer = ref.ref_data.robot_power_heat.buffer_energy;
-        float power_limit_ratio = 1.0;
-        float power_buffer_limit_thresh = power_buffer_controller.gains.power_buffer_threshold;
-        float power_buffer_critical_thresh = power_buffer_controller.gains.power_buffer_critical_threshold;
-        if (power_buffer < power_buffer_limit_thresh) {
-            power_limit_ratio = constrain(((power_buffer - power_buffer_critical_thresh) / power_buffer_limit_thresh), 0.0, 1.0);
-        }
+        float power_limit_ratio = compute_power_limit_ratio(
+            ref.ref_data.robot_power_heat.buffer_energy,
+            power_buffer_controller.gains.power_buffer_threshold,
+            power_buffer_controller.gains.power_buffer_critical_threshold
+        );
 
         float motor_outputs[4];
 
@@ -248,8 +248,7 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat
         outputp[2] = pidp[2].filter(dt, false, false);
         outputv[2] = pidv[2].filter(dt, false, false);
         output[2] = (outputp[2] + outputv[2]) * controller_config.gear_ratios.chassis_rad_to_motor_rad;
-        // Serial.printf("chassis heading output: %f, chassis x output: %f, chassis y output: %f chassis angle:%f\n", output[2], output[0], output[1], estimate[2][0]);
-        // Adjust for chassis heading so control is field relative
+
         float chassis_heading = estimate_map[Cfg::StateName::ChassisHeading].get_position();
 
         // Convert to motor velocities
@@ -258,13 +257,11 @@ void XDriveController::step(RobotStateMap& reference_map, RobotStateMap& estimat
         motor_velocity[3] = -output[0] * cos(chassis_heading) - output[1] * sin(chassis_heading) + output[2];
         motor_velocity[0] = -output[0] * sin(chassis_heading) + output[1] * cos(chassis_heading) + output[2];
         // Power limiting
-        float power_buffer = ref.ref_data.robot_power_heat.buffer_energy;
-        float power_limit_ratio = 1.0;
-        float power_buffer_limit_thresh = power_buffer_controller.gains.power_buffer_threshold;
-        float power_buffer_critical_thresh = power_buffer_controller.gains.power_buffer_critical_threshold;
-        if (power_buffer < power_buffer_limit_thresh) {
-            power_limit_ratio = constrain(((power_buffer - power_buffer_critical_thresh) / power_buffer_limit_thresh), 0.0, 1.0);
-        }
+        float power_limit_ratio = compute_power_limit_ratio(
+            ref.ref_data.robot_power_heat.buffer_energy,
+            power_buffer_controller.gains.power_buffer_threshold,
+            power_buffer_controller.gains.power_buffer_critical_threshold
+        );
 
         float motor_outputs[4];
 
@@ -313,11 +310,10 @@ void YawController::step(RobotStateMap& reference_map, RobotStateMap& estimate_m
     pidv.setpoint = reference_map[yaw_angle_state].get_velocity();
     pidv.measurement = estimate_map[yaw_angle_state].get_velocity();
 
-    output += pidp.filter(dt, true, true); // position wraps
-    output += pidv.filter(dt, true, false); // no wrap for velocity
-
-    output = constrain(output, -1.0, 1.0);
+    output += pidp.filter(dt, true, true);
+    output += pidv.filter(dt, true, false);
 
+    output = clamp1(output);
 
     float motor_outputs[2];
 
@@ -352,13 +348,12 @@ void PitchController::step(RobotStateMap& reference_map, RobotStateMap& estimate
     pidp.setpoint = reference_map[pitch_angle_state].get_position();
     pidp.measurement = estimate_map[pitch_angle_state].get_position();
 
-
     pidv.setpoint = reference_map[pitch_angle_state].get_velocity();
     pidv.measurement = estimate_map[pitch_angle_state].get_velocity();
 
-    output += pidp.filter(dt, true, false); // position wraps
-    output += pidv.filter(dt, true, false); // no wrap for velocity
-    output = constrain(output, -1.0, 1.0);
+    output += pidp.filter(dt, true, false);
+    output += pidv.filter(dt, true, false);
+    output = clamp1(output);
 
     float motor_outputs[2]; 
 
@@ -404,7 +399,7 @@ void FlywheelController::step(RobotStateMap& reference_map, RobotStateMap& estim
 
         pid_low.setpoint = target_motor_velocity * flywheel_directions[i];
         pid_low.measurement = flywheel_motors[i]->get_state().speed;
-        
+
         flywheel_motors[i]->write_motor_torque(pid_low.filter(dt, true, false));
     }
 }
@@ -428,14 +423,14 @@ void FeederController::step(RobotStateMap& reference_map, RobotStateMap& estimat
     pidv.ki = full_state_velocity_controller.gains.i;
     pidv.kd = full_state_velocity_controller.gains.d;
     pidv.kf = full_state_velocity_controller.gains.f;
-    
+
     pidp.setpoint = reference_map[feeder_position_state].get_position();
     pidp.measurement = estimate_map[feeder_position_state].get_position();
 
     float outputp = pidp.filter(dt, true, true);
     float output = outputp * controller_config.gear_ratios.feeder_direction;
 
-   feeder_motor->write_motor_torque(output);
+    feeder_motor->write_motor_torque(output);
 }
 
 void FeederController::handleControllerError(const char* controller_name, const char* state_name, const State& reference_state, const State& estimate_state, float error) {
@@ -494,9 +489,9 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es
         // Serial.printf("upper feeder shot, time: %f \n", (micros() - target_increase_time) / 1000.0);
         timer_active = false;
     }
-    
+
     upper_feeder_reference_state = upper_feeder_reference_governor.step_reference_map(upper_target);
-    
+
     upper_pidp.setpoint = upper_feeder_reference_state[upper_feeder_position_state].get_position();
     upper_pidp.measurement = upper_pos;
 
@@ -508,7 +503,7 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es
 
     lower_pidv.setpoint = reference_map[upper_feeder_position_state].get_velocity();
     lower_pidv.measurement = estimate_map[lower_feeder_position_state].get_velocity();
-    
+
     float upper_outputp = upper_pidp.filter(dt, true, true);
     float upper_outputv = upper_pidv.filter(dt, true, false);
     float upper_output = (upper_outputp + upper_outputv) * controller_config.gear_ratios.upper_feeder_direction;
@@ -518,13 +513,13 @@ void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& es
     float lower_outputv = lower_pidv.filter(dt, true, false);
     float lower_output = (lower_outputp + lower_outputv) * controller_config.gear_ratios.lower_feeder_direction;
     lower_output = constrain(lower_output, -1.0, 1.0);
-    
+
     // Serial.printf("Feeder Velocity Setpoint: %f, Measurement: %f, output: %f\n", lower_pidv.setpoint, lower_pidv.measurement, output);
-    // Serial.printf("lower feeder reference position: %f, reference velocity: %f, estimate position: %f, estimate velocity: %f\n", 
+    // Serial.printf("lower feeder reference position: %f, reference velocity: %f, estimate position: %f, estimate velocity: %f\n",
     //                 reference_map[lower_feeder_position_state].get_position(), reference_map[lower_feeder_position_state].get_velocity(),
     //                 estimate_map[lower_feeder_position_state].get_position(), estimate_map[lower_feeder_position_state].get_velocity());
-    upper_feeder_motor->write_motor_torque(upper_output);    
+    upper_feeder_motor->write_motor_torque(upper_output);
 
     near_feeder_motor->write_motor_torque(lower_output);
     far_feeder_motor->write_motor_torque(-lower_output);
-}
+}

src/controls/controller.hpp

@@ -1,9 +1,10 @@
 #pragma once
 
+#include "controller_math.hpp"
 #include "estimator.hpp"
 #include "filters/pid_filter.hpp"
-#include "motor.hpp"
-#include "safety.hpp"
+#include "sensors/can/motor.hpp"
+#include "utils/safety.hpp"
 #include "utils/timing.hpp"
 #include "sensors/can/can_manager.hpp"
 #include "robot_state_map.hpp"
@@ -573,4 +574,4 @@ struct LowerFeederController : public Controller {
             lower_pidv.sumError = 0.0;
             lower_feeder_error_monitor = ErrorMonitor{};
         }
-};
+};

src/controls/controller_math.cpp

@@ -0,0 +1,26 @@
+#include "controller_math.hpp"
+
+#include <algorithm>
+#include <cmath>
+
+float compute_power_limit_ratio(float buffer, float limit_thresh, float critical_thresh) {
+    if (buffer >= limit_thresh) {
+        return 1.0f;
+    }
+    return std::clamp((buffer - critical_thresh) / limit_thresh, 0.0f, 1.0f);
+}
+
+MotorVelocities xdrive_mix(float x, float y, float rot, float heading) {
+    const float cosine = std::cos(heading);
+    const float sine = std::sin(heading);
+    return {{
+        x * cosine + y * sine + rot,
+        x * sine - y * cosine + rot,
+        -x * cosine - y * sine + rot,
+        -x * sine + y * cosine + rot,
+    }};
+}
+
+float clamp1(float value) {
+    return std::clamp(value, -1.0f, 1.0f);
+}

src/controls/controller_math.hpp

@@ -0,0 +1,11 @@
+#pragma once
+
+/// @brief Holds the normalized velocity outputs for all four motors of an X-drive chassis.
+struct MotorVelocities {
+    /// @brief Normalized velocity for each motor, indexed 0-3.
+    float v[4];
+};
+
+float compute_power_limit_ratio(float buffer, float limit_thresh, float critical_thresh);
+MotorVelocities xdrive_mix(float x, float y, float rot, float heading);
+float clamp1(float value);

src/controls/estimator.cpp

@@ -2,7 +2,7 @@
 #include <limits>
 
 #include "estimator.hpp"
-#include "ICM20649.hpp"
+#include "sensors/ICM20649.hpp"
 #include "utils/vector_math.hpp"
 #include "utils/wrapping.hpp"
 #include "sensors/RefSystem.hpp"

src/controls/estimator.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
-#include "estimator.hpp"
+#include "comms/config_data/estimator.hpp"
 #include "robot_state_map.hpp"
-#include "safety.hpp"
+#include "utils/safety.hpp"
 #include "sensors/can/can_manager.hpp"
 #include "state.hpp"
 #include "utils/timing.hpp"

src/hello_robot.hpp

@@ -2,16 +2,16 @@
 #include <Arduino.h>
 #include <optional>
 
-#include "can_manager.hpp"
+#include "sensors/can/can_manager.hpp"
 #include "comms/comms_layer.hpp"
 #include "controls/reference_governor.hpp"
 #include "controls/state.hpp"
 #include "git_info.h"
 
-#include "robot_state_map.hpp"
-#include "safety.hpp"
+#include "controls/robot_state_map.hpp"
+#include "utils/safety.hpp"
 #include "sensors/buff_encoder.hpp"
-#include "state.hpp"
+#include "comms/config_data/state.hpp"
 #include "utils/boot_splash.hpp"
 #include "utils/profiler.hpp"
 
@@ -25,7 +25,7 @@
 #include "sensors/StereoCamTrigger.hpp"
 #include "utils/profiler.hpp"
 
-#include "sensor_manager.hpp"
+#include "sensors/sensor_manager.hpp"
 #include <TeensyDebug.h>
 #include <wiring.h>
 

src/sensors/ICM20649.cpp

@@ -1,5 +1,5 @@
 #include "ICM20649.hpp"
-#include "safety.hpp"
+#include "utils/safety.hpp"
 #include "comms/data/sendable.hpp"
 
 // initialize ICM

src/sensors/RefSystem.cpp

@@ -3,9 +3,6 @@
 #include "comms/data/sendable.hpp"
 RefSystem ref; // Global instance
 
-// Uncomment to enable debug prints
-// #define REF_SYSTEM_DEBUG
-
 uint8_t generateCRC8(const uint8_t *data, uint32_t len) {
     uint8_t CRC8 = 0xFF;
     while (len-- > 0) {

src/sensors/StereoCamTrigger.cpp

@@ -1,6 +1,6 @@
 #include "StereoCamTrigger.hpp"
 #include "comms/data/sendable.hpp"
-#include "transmitter_utils.hpp"
+#include "sensors/transmitter/transmitter_utils.hpp"
 #include <core_pins.h>
 
 std::unique_ptr<RobotStateMap>* StereoCamTrigger::estimated_state_map_interrupt_safe = nullptr;

src/sensors/StereoCamTrigger.hpp

@@ -4,7 +4,7 @@
 #include <avr/interrupt.h>
 #include "sensors/sensor.hpp"
 #include "comms/data/stereo_cam_trigger_data.hpp"
-#include "robot_state_map.hpp"
+#include "controls/robot_state_map.hpp"
 #include <memory>