diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index 590a9d52e..0a4991b64 100644 --- a/include/MCU_rev15_defs.h +++ b/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; diff --git a/lib/mock_interfaces/SteeringEncoderInterface.h b/lib/mock_interfaces/SteeringEncoderInterface.h index bef37fc69..09a710834 100644 --- a/lib/mock_interfaces/SteeringEncoderInterface.h +++ b/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__ */ diff --git a/lib/systems/include/CASESystem.h b/lib/systems/include/CASESystem.h index d703c4176..30449f33a 100644 --- a/lib/systems/include/CASESystem.h +++ b/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; diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 8c4f5f638..8c05abba8 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -143,6 +143,15 @@ DrivetrainCommand_s CASESystem::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::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; } diff --git a/lib/systems/include/SteeringSystem.h b/lib/systems/include/SteeringSystem.h index c2f7e340e..75ade62b8 100644 --- a/lib/systems/include/SteeringSystem.h +++ b/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 diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp index e798b7748..32439736e 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/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(); } } diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index fe39c1e27..96b8b705f 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -88,10 +88,13 @@ void TorqueControllerMux::tick( drivetrainCommand_ = controllerOutputs_[static_cast(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_); } } @@ -99,6 +102,12 @@ void TorqueControllerMux::tick( /* 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++) diff --git a/platformio.ini b/platformio.ini index c4ad878f5..d827a52e2 100644 --- a/platformio.ini +++ b/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 = diff --git a/src/main.cpp b/src/main.cpp index 9cfef96de..0e71e20df 100644 --- a/src/main.cpp +++ b/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(); + } } /* diff --git a/test/test_systems/main.cpp b/test/test_systems/main.cpp index 8a56a607e..8038eb36d 100644 --- a/test/test_systems/main.cpp +++ b/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) { diff --git a/test/test_systems/steering_system_test.h b/test/test_systems/steering_system_test.h new file mode 100644 index 000000000..9b0d22fa5 --- /dev/null +++ b/test/test_systems/steering_system_test.h @@ -0,0 +1,202 @@ +#ifndef STEERING_SYSTEM_TEST +#define STEERING_SYSTEM_TEST +#include +#include +#include "SteeringSystem.h" + +TEST(SteeringSystemTesting, test_steering_nominal) +{ + float angles_to_test[5] = {-120.0f, -60.0f, 0.0f, 60.0f, 120.0f}; + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + + // Sweep through a few angles where the sensors agree perfectly and check the system is nominal + for (float angle: angles_to_test) + { + primarySensor.mockConversion.angle = angle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL); + } +} + +TEST(SteeringSystemTesting, test_steering_primary_is_marginal) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + float angle = 0.0f; + + primarySensor.mockConversion.angle = angle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); +} + +TEST(SteeringSystemTesting, test_steering_secondary_is_marginal) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + float angle = 0.0f; + + primarySensor.mockConversion.angle = angle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); +} + +TEST(SteeringSystemTesting, test_steering_divergence_warning) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + float primaryAngle = 0.0f; + float secondaryAngle = primaryAngle + STEERING_DIVERGENCE_WARN_THRESHOLD; + + primarySensor.mockConversion.angle = primaryAngle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = secondaryAngle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == primaryAngle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL); +} + +TEST(SteeringSystemTesting, test_steering_divergence_error) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + float primaryAngle = 0.0f; + float secondaryAngle = primaryAngle + STEERING_DIVERGENCE_ERROR_THRESHOLD; + + primarySensor.mockConversion.angle = primaryAngle; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = secondaryAngle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == 0.0f); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR); +} + +TEST(SteeringSystemTesting, test_steering_primary_is_missing) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + float angle = 100.0f; + + primarySensor.mockConversion.angle = 0.0f; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == angle); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED); +} + +TEST(SteeringSystemTesting, test_steering_primary_is_missing_and_secondary_is_marginal) +{ + // Boilerplate definitions for the testing aparatus + SysClock sys_clock; + SysTick_s sys_tick = sys_clock.tick(0); + SteeringEncoderInterface primarySensor; + SteeringSystem steeringSystem(&primarySensor); + float angle = 100.0f; + + primarySensor.mockConversion.angle = 0.0f; + primarySensor.mockConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR; + steeringSystem.tick( + (SteeringSystemTick_s) + { + .tick = sys_tick, + .secondaryConversion = (AnalogConversion_s) + { + .raw = 0, + .conversion = angle, + .status = AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED + } + } + ); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().angle == 0.0f); + ASSERT_TRUE(steeringSystem.getSteeringSystemData().status == SteeringSystemStatus_e::STEERING_SYSTEM_ERROR); +} + +#endif \ No newline at end of file