Merge branch 'CASE_testbranch' into feature/filter-steering-reading

include/MCU_rev15_defs.h

@@ -98,7 +98,7 @@ const float LOADCELL_RR_SCALE = 0.06;
 const float LOADCELL_RR_OFFSET = 23.761 / LOADCELL_RR_SCALE;
 
 // Steering parameters
-const float PRIMARY_STEERING_SENSE_OFFSET = -21.18; // units are degrees
+const float PRIMARY_STEERING_SENSE_OFFSET = 0.0; // units are degrees
 const int SECONDARY_STEERING_SENSE_LEFTMOST_BOUND = 812;
 const int SECONDARY_STEERING_SENSE_RIGHTMOST_BOUND = 3179;
 const int SECONDARY_STEERING_SENSE_CENTER = 1970;

lib/mock_interfaces/SteeringEncoderInterface.h

@@ -20,15 +20,18 @@ struct SteeringEncoderConversion_s
 class SteeringEncoderInterface
 {
 public:
+// Mock data
+    SteeringEncoderConversion_s mockConversion;
 // Functions
-    /// @brief Commands the underlying steering sensor to sample and hold the result
-    virtual void sample();
-    /// @brief Calculate steering angle and whether result is in sensor's defined bounds. DOES NOT SAMPLE.
-    /// @return Calculated steering angle in degrees, upperSteeringStatus_s
-    virtual SteeringEncoderConversion_s convert();
-    /// @brief Set the upper steering sensor's offset. 0 degrees should be centered.
-    /// @param newOffset 
-    virtual void setOffset(float newOffset);
+    void sample()
+    {
+        return;
+    };
+    SteeringEncoderConversion_s convert()
+    {
+        return mockConversion;
+    };
+    void setOffset(float newOffset);
 };
 
 #endif /* __UPPERSTEERINGSENSOR_H__ */

lib/systems/include/CASESystem.h

@@ -64,6 +64,12 @@ struct CASEConfiguration
     float TCSGenLeftRightDiffUpperBound;
     float TCSWheelSteerLowerBound;
     float TCSWheelSteerUpperBound;
+    bool useRPM_TCS_GainSchedule;
+    bool useNL_TCS_GainSchedule;
+    float TCS_NL_startBoundPerc_FrontAxle;
+    float TCS_NL_endBoundPerc_FrontAxle;
+    float TCS_NL_startBoundPerc_RearAxle;
+    float TCS_NL_endBoundPerc_RearAxle;
 
     float max_rpm;
     float max_regen_torque;

lib/systems/include/CASESystem.tpp

@@ -143,6 +143,15 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
 
     in.TCSWheelSteerUpperBound = config_.TCSWheelSteerUpperBound;
 
+    in.useRPM_TCS_GainSchedule = config_.useRPM_TCS_GainSchedule;
+
+    in.useNL_TCS_GainSchedule = config_.useNL_TCS_GainSchedule;
+
+    in.TCS_PID_NL_Schedule[0] = config_.TCS_NL_startBoundPerc_FrontAxle;
+    in.TCS_PID_NL_Schedule[1] = config_.TCS_NL_endBoundPerc_FrontAxle;
+    in.TCS_PID_NL_Schedule[2] = config_.TCS_NL_startBoundPerc_RearAxle;
+    in.TCS_PID_NL_Schedule[3] = config_.TCS_NL_endBoundPerc_RearAxle;
+
     if ((vn_active_start_time_ == 0) && (vn_status >= 2))
     {
         vn_active_start_time_ = tick.millis;
@@ -226,6 +235,8 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
         enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_yaw_pi);
         enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_sa);
         enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_di);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_rp);
+        enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_tcs_nl);
 
         last_lowest_priority_controller_send_time_ = tick.millis;
     }

lib/systems/include/SteeringSystem.h

@@ -10,9 +10,8 @@
 
 // Definitions
 // TODO: evalaute reasonable thresholds for agreement
-#define STEERING_DIVERGENCE_ERROR_THRESHOLD (14.0) // Steering sensors can disagree by 5 degrees before output is considered erroneous
-#define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge 2.5 degrees
-#define NUM_SENSORS (2)
+#define STEERING_DIVERGENCE_ERROR_THRESHOLD (14.0) // Steering sensors can disagree by x degrees before output is considered erroneous
+#define STEERING_DIVERGENCE_WARN_THRESHOLD (8.0) // Warning condition will be raised when steering sensors diverge x degrees
 
 // Enums
 enum class SteeringSystemStatus_e

lib/systems/src/SteeringSystem.cpp

@@ -32,7 +32,7 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
         // Sensors disagree by STEERING_DIVERGENCE_WARN_THRESHOLD degrees and less than STEERING_DIVERGENCE_ERROR_THRESHOLD degrees
         else if (
             (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL)
-            || (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED)
+            || ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED))
             || ((std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_ERROR_THRESHOLD))
         )
         {
@@ -61,5 +61,11 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
                 .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR
             };
         }
+
+        // For possible bottom sensor recalibration
+        // TODO: Remove me once done!
+        // Serial.print("Secondary sensor raw: ");
+        // Serial.println(intake.secondaryConversion.raw);
+        // Serial.println();
     }
 }

lib/systems/src/TorqueControllerMux.cpp

@@ -88,17 +88,26 @@ void TorqueControllerMux::tick(
 
         drivetrainCommand_ = controllerOutputs_[static_cast<int>(muxMode_)].command;
 
-        // apply torque limit before power limit to not power limit
-        // applyRegenLimit(&drivetrainCommand_, &drivetrainData);
-        // applyTorqueLimit(&drivetrainCommand_);
-        // applyPowerLimit(&drivetrainCommand_, &drivetrainData);
+        // Apply setpoints value limits
+        // Safety checks for CASE: CASE handles regen, torque, and power limit internally
+        applyRegenLimit(&drivetrainCommand_, &drivetrainData);
+        // Apply torque limit before power limit to not power limit
+        applyTorqueLimit(&drivetrainCommand_);
+        applyPowerLimit(&drivetrainCommand_, &drivetrainData);
+        // Uniformly apply speed limit to all controller modes
         applyPosSpeedLimit(&drivetrainCommand_);
     }
 }
 
 /*
     Apply limit to make sure that regenerative braking is not applied when
     wheelspeed is below 5kph on all wheels.
+
+    FSAE rules:
+        EV.3.3.3 The powertrain must not regenerate energy when vehicle speed is between 0 and 5 km/hr
+    Assumption:
+        Assuming there won't be a scenario where there are positive and negative setpoints simultaneously
+        AND vehicle speed is < 5km/h
 */
 void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const DrivetrainDynamicReport_s *drivetrain)
 {
@@ -217,7 +226,9 @@ void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s *command)
     }
 }
 
-/* Apply limit such that wheelspeed never goes negative */
+/**
+ *  Apply limit such that wheelspeed never goes negative
+ */
 void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s *command)
 {
     for (int i = 0; i < NUM_MOTORS; i++)

platformio.ini

@@ -43,6 +43,7 @@ check_src_filters =
 platform = teensy
 board = teensy41
 framework = arduino
+monitor_speed = 115200
 upload_protocol = teensy-cli
 extra_scripts = pre:extra_script.py
 lib_deps = 
@@ -55,8 +56,8 @@ lib_deps =
     https://github.com/RCMast3r/spi_libs
     https://github.com/tonton81/FlexCAN_T4
     https://github.com/RCMast3r/hytech_can#testing_new_inv_ids
-    https://github.com/hytech-racing/HT_CAN/releases/download/89/can_lib.tar.gz
-    git+ssh://git@github.com/hytech-racing/CASE_lib.git#v47
+    https://github.com/hytech-racing/HT_CAN/releases/download/96/can_lib.tar.gz
+    git+ssh://git@github.com/hytech-racing/CASE_lib.git#v48
 
 [env:test_can_on_teensy]
 lib_ignore =

src/main.cpp

@@ -6,7 +6,7 @@
 #include "FlexCAN_T4.h"
 #include "HyTech_CAN.h"
 #include "MCU_rev15_defs.h"
-#include "NativeEthernet.h"
+// #include "NativeEthernet.h"
 
 // /* Interfaces */
 
@@ -157,8 +157,6 @@ CASEConfiguration case_config = {
     .AbsoluteTorqueLimit = 21.42, // N-m, Torque limit used for yaw pid torque split overflow
     .yaw_pid_p = 1.5,
     .yaw_pid_i = 0.25,
-    // .yaw_pid_p = 0.5, // SKIDPAD Testing
-    // .yaw_pid_i = 0.0, // SKIDPAD Testing
     .yaw_pid_d = 0.0,
     .tcs_pid_p_lowerBound_front = 35.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error
     .tcs_pid_p_upperBound_front = 45.0,
@@ -179,7 +177,7 @@ CASEConfiguration case_config = {
     .launchSL = 0.3,
     .launchDeadZone = 20.0,        // N-m
     .launchVelThreshold = 0.15,    // m/s
-    .tcsVelThreshold = 0.15,       // m/s
+    .tcsVelThreshold = 1.5,        // m/s
     .yawPIDMaxDifferential = 10.0, // N-m
     .yawPIDErrorThreshold = 0.1,   // rad/s
     .yawPIDVelThreshold = 0.35,    // m/s
@@ -208,6 +206,12 @@ CASEConfiguration case_config = {
     .TCSGenLeftRightDiffUpperBound = 20, // N-m
     .TCSWheelSteerLowerBound = 2,        // Deg
     .TCSWheelSteerUpperBound = 25,       // Deg
+    .useRPM_TCS_GainSchedule = false,    // If both are false, then P values defaults to lower bound per axle
+    .useNL_TCS_GainSchedule = true,
+    .TCS_NL_startBoundPerc_FrontAxle = 0.5,
+    .TCS_NL_endBoundPerc_FrontAxle = 0.4,
+    .TCS_NL_startBoundPerc_RearAxle = 0.5,
+    .TCS_NL_endBoundPerc_RearAxle = 0.6,
 
     // Following used for calculate_torque_request in CASESystem.tpp
     .max_rpm = 20000,
@@ -349,6 +353,15 @@ void loop()
     // send CAN
     send_all_CAN_msgs(CAN2_txBuffer, &INV_CAN);
     send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN);
+
+    // Basic debug prints
+    if (curr_tick.triggers.trigger5)
+    {
+        Serial.print("Steering system reported angle (deg): ");
+        Serial.println(steering_system.getSteeringSystemData().angle);
+
+        Serial.println();
+    }
 }
 
 /*

test/test_systems/main.cpp

@@ -7,6 +7,7 @@
 #include "safety_system_test.h"
 // #include "test_CASE.h"
 // #include "param_system_test.h"
+#include "steering_system_test.h"
 
 int main(int argc, char **argv)
 {