diff --git a/lib/systems/include/PhysicalParameters.h b/lib/systems/include/PhysicalParameters.h index c090d6846..7f971575a 100644 --- a/lib/systems/include/PhysicalParameters.h +++ b/lib/systems/include/PhysicalParameters.h @@ -1,9 +1,13 @@ #ifndef __PHYSICALPARAMETERS_H__ #define __PHYSICALPARAMETERS_H__ -const float GEARBOX_RATIO = 11.86; -const float WHEEL_DIAMETER = 0.4064; // meters -const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; -const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; +const float GEARBOX_RATIO = 11.86; +const float WHEEL_DIAMETER = 0.4064; // meters +const float RPM_TO_METERS_PER_SECOND = WHEEL_DIAMETER * 3.1415 / GEARBOX_RATIO / 60.0; +const float RPM_TO_KILOMETERS_PER_HOUR = RPM_TO_METERS_PER_SECOND * 3600.0 / 1000.0; +const float METERS_PER_SECOND_TO_RPM = 1.0 / RPM_TO_METERS_PER_SECOND; + +const float RPM_TO_RAD_PER_SECOND = 2 * 3.1415 / 60.0; +const float RAD_PER_SECOND_TO_RPM = 1 / RPM_TO_RAD_PER_SECOND; #endif /* __PHYSICALPARAMETERS_H__ */ \ No newline at end of file diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 01e0185d8..8f3410a0a 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -2,6 +2,8 @@ #define __TORQUECTRLMUX_H__ #include +#include + #include "TorqueControllers.h" #include "DrivetrainSystem.h" #include "PedalsSystem.h" @@ -29,7 +31,7 @@ class TorqueControllerMux std::unordered_map torqueLimitMap_ = { {TorqueLimit_e::TCMUX_LOW_TORQUE, 10.0}, {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, - {TorqueLimit_e::TCMUX_FULL_TORQUE, 21.4} + {TorqueLimit_e::TCMUX_FULL_TORQUE, AMK_MAX_TORQUE} }; TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; @@ -72,7 +74,7 @@ class TorqueControllerMux TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale) : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) - , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) + , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale) , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SIMPLE_LAUNCH)]) , torqueControllerSlipLaunch_(controllerOutputs_[static_cast(TorqueController_e::TC_SLIP_LAUNCH)]) , torqueControllerPIDTV_(controllerOutputs_[static_cast(TorqueController_e::TC_PID_VECTORING)]) {} @@ -90,18 +92,31 @@ class TorqueControllerMux bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, float wheel_angle_rad); + const DrivetrainCommand_s &getDrivetrainCommand() { return drivetrainCommand_; }; + const TorqueLimit_e &getTorqueLimit() { return torqueLimit_; }; + const float getMaxTorque() { return torqueLimitMap_[torqueLimit_]; } + + void applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); + + void applyTorqueLimit(DrivetrainCommand_s* command); + + void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); + + void applyPosSpeedLimit(DrivetrainCommand_s* command); + + TorqueControllerBase* activeController() { // check to make sure that there is actually a controller diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index af757d841..151c95705 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -15,6 +15,10 @@ #include "TorqueControllersData.h" #include "PID_TV.h" +/* CONTROLLER CONSTANTS */ + +const float MAX_POWER_LIMIT = 63000.0; // max mechanical power limit in KW + /* MOTOR CONSTANTS */ const float AMK_MAX_RPM = 20000; @@ -120,22 +124,6 @@ class TorqueController : public TorqueControllerBase { protected: - void TCPowerLimitScaleDown( - DrivetrainCommand_s &command, - const DrivetrainDynamicReport_s &drivetrainData, - float powerLimit) - { - // TODO - // probably requires AMS interface - } - void TCPosTorqueLimit(DrivetrainCommand_s &command, float torqueLimit) - { - for (int i = 0; i < NUM_MOTORS; i++) - { - command.torqueSetpoints[i] = std::min(command.torqueSetpoints[i], torqueLimit); - } - } - public: }; @@ -161,11 +149,10 @@ class TorqueControllerSimple : public TorqueController float rearRegenTorqueScale_ = 1.0; public: - /// @brief simple TC in which a scaling can be applied to both regen and accel torques for scaling accel request (accel percent - regen percent) + /// @brief simple TC with tunable F/R torque balance. Accel torque balance can be tuned independently of regen torque balance /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command - /// @param rearTorqueScale the 0 to 2 scaling with which 0 represents 200 percent of the accel percent with which to request torque from front wheels, 2 being vice versa to the rear and 1 being balanced. - /// @param regenTorqueScale same as rearTorqueScale, accept applied to negative accel percents which correspond to regen - + /// @param rearTorqueScale 0 to 2 scale on forward torque to rear wheels. 0 = FWD, 1 = Balanced, 2 = RWD + /// @param regenTorqueScale same as rearTorqueScale but applies to regen torque split. 0 = All regen torque on the front, 1 = 50/50, 2 = all regen torque on the rear TorqueControllerSimple(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) : writeout_(writeout), frontTorqueScale_(2.0 - rearTorqueScale), @@ -187,6 +174,8 @@ class TorqueControllerLoadCellVectoring : public TorqueController(muxMode_)].command; + + // apply torque limit before power limit to not power limit + applyRegenLimit(&drivetrainCommand_, &drivetrainData); + applyTorqueLimit(&drivetrainCommand_); + applyPowerLimit(&drivetrainCommand_, &drivetrainData); + applyPosSpeedLimit(&drivetrainCommand_); + + } +} + +/* + Apply limit to make sure that regenerative braking is not applied when + wheelspeed is below 5kph on all wheels. +*/ +void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) +{ + const float noRegenLimitKPH = 10.0; + const float fullRegenLimitKPH = 5.0; + float maxWheelSpeed = 0.0; + float torqueScaleDown = 0.0; + bool allWheelsRegen = true; // true when all wheels are targeting speeds below the current wheel speed + + for (int i = 0; i < NUM_MOTORS; i++) { + maxWheelSpeed = std::max(maxWheelSpeed, drivetrain->measuredSpeeds[i] * RPM_TO_KILOMETERS_PER_HOUR); + allWheelsRegen &= (command->speeds_rpm[i] < drivetrain->measuredSpeeds[i]); + } + + // begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH + // linearly interpolate the scale factor between noRegenLimitKPH and fullRegenLimitKPH + torqueScaleDown = std::min(1.0f, std::max(0.0f, (maxWheelSpeed - fullRegenLimitKPH) / (noRegenLimitKPH - fullRegenLimitKPH))); + + if (allWheelsRegen) + { + for (int i = 0; i < NUM_MOTORS; i++) { + command->torqueSetpoints[i] *= torqueScaleDown; + } + } +} + +/* + Apply power limit such that the mechanical power of all wheels never + exceeds the preset mechanical power limit. Scales all wheels down to + preserve functionality of torque controllers +*/ +void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) +{ + float net_torque_mag = 0; + float net_power = 0; + + // calculate current mechanical power + for (int i = 0; i < NUM_MOTORS; i++) { + // get the total magnitude of torque from all 4 wheels + #ifdef ARDUINO_TEENSY41 // screw arduino.h macros + net_torque_mag += abs(command->torqueSetpoints[i]); + net_power += abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + #else + // sum up net torque + net_torque_mag += std::abs(command->torqueSetpoints[i]); + // calculate P = T*w for each wheel and sum together + net_power += std::abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND)); + #endif + } + + // only evaluate power limit if current power exceeds it + if (net_power > (MAX_POWER_LIMIT)) { + for (int i = 0; i < NUM_MOTORS; i++) { + // calculate the percent of total torque requested per wheel + float torque_percent = command->torqueSetpoints[i] / net_torque_mag; + // based on the torque percent and max power limit, get the max power each wheel can use + float power_per_corner = (torque_percent * MAX_POWER_LIMIT); + // power / omega (motor rad/s) to get torque per wheel + command->torqueSetpoints[i] = power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND); + } + } + +} + +/* + Apply limit such that the average wheel torque is never above the max + torque allowed in the current torque mode. + This will uniformally scale down all torques as to not affect the functionality + of non-symmetrical torque controllers. +*/ +void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s* command) +{ + float max_torque = getMaxTorque(); + float avg_torque = 0; + + // get the average torque accross all 4 wheels + for (int i = 0; i < NUM_MOTORS; i++) { + #ifdef ARDUINO_TEENSY41 // screw arduino.h macros + avg_torque += abs(command->torqueSetpoints[i]); + #else + avg_torque += std::abs(command->torqueSetpoints[i]); + #endif + } + avg_torque /= NUM_MOTORS; + + // if this is greather than the torque limit, scale down + if (avg_torque > max_torque) { + // get the scale of avg torque above max torque + float scale = avg_torque / max_torque; + // divide by scale to lower avg below max torque + for (int i = 0; i < NUM_MOTORS; i++) { + command->torqueSetpoints[i] /= scale; + } + } + +} + +/* Apply limit such that wheelspeed never goes negative */ +void TorqueControllerMux::applyPosSpeedLimit(DrivetrainCommand_s* command) { + for (int i = 0; i < NUM_MOTORS; i++) + { + command->torqueSetpoints[i] = std::max(0.0f, command->torqueSetpoints[i]); } } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index d4fbacf2e..229398b1e 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -50,9 +50,6 @@ void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_ writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; } - - // Apply the torque limit - TCPosTorqueLimit(writeout_.command, torqueLimit); } } @@ -140,14 +137,11 @@ void TorqueControllerLoadCellVectoring::tick( writeout_.command.speeds_rpm[RL] = 0.0; writeout_.command.speeds_rpm[RR] = 0.0; - writeout_.command.torqueSetpoints[FL] = torqueRequest; - writeout_.command.torqueSetpoints[FR] = torqueRequest; - writeout_.command.torqueSetpoints[RL] = torqueRequest; - writeout_.command.torqueSetpoints[RR] = torqueRequest; + writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; } - - // Apply the torque limit - TCPosTorqueLimit(writeout_.command, torqueLimit); } else { diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index fbd85bcf8..5283e3eec 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -287,6 +287,77 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) } } +TEST(TorqueControllerMuxTesting, test_power_limit) { + TorqueControllerMux mux = TorqueControllerMux(); + DrivetrainCommand_s drive_command; + + DrivetrainDynamicReport_s edit; + + for (int i = 0; i < 4; i++) { + edit.measuredSpeeds[i] = 500.0f; + drive_command.torqueSetpoints[i] = 10.0f; + } + + const DrivetrainDynamicReport_s drivetrain1 = edit; + + mux.applyPowerLimit(&drive_command, &drivetrain1); + + for (int i = 0; i < 4; i++) { + ASSERT_EQ(drive_command.torqueSetpoints[i], 10.0f); + } + + for (int i = 0; i < 4; i++) { + edit.measuredSpeeds[i] = 20000.0f; + drive_command.torqueSetpoints[i] = 21.0f; + } + + const DrivetrainDynamicReport_s drivetrain2 = edit; + + mux.applyPowerLimit(&drive_command, &drivetrain2); + + for (int i = 0; i < 4; i++) { + ASSERT_LT(drive_command.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator + } + +} + +TEST(TorqueControllerMuxTesting, test_torque_limit) { + TorqueControllerMux mux = TorqueControllerMux(); + DrivetrainCommand_s drive_command; + + for (int i = 0; i < 4; i++) { + drive_command.speeds_rpm[i] = 500.0f; + drive_command.torqueSetpoints[i] = 10.0f; + } + + drive_command.torqueSetpoints[0] = 5; + + mux.applyTorqueLimit(&drive_command); + + ASSERT_EQ(drive_command.torqueSetpoints[0], 5.0f); + ASSERT_EQ(drive_command.torqueSetpoints[1], 10.0f); + ASSERT_EQ(drive_command.torqueSetpoints[2], 10.0f); + ASSERT_EQ(drive_command.torqueSetpoints[3], 10.0f); + + for (int i = 0; i < 4; i++) { + drive_command.speeds_rpm[i] = 500.0f; + drive_command.torqueSetpoints[i] = 20.0f; + } + drive_command.torqueSetpoints[0] = 5; + + mux.applyTorqueLimit(&drive_command); + + ASSERT_LT(drive_command.torqueSetpoints[0], 3.5f); + ASSERT_LT(drive_command.torqueSetpoints[1], 12.5f); + ASSERT_LT(drive_command.torqueSetpoints[2], 12.5f); + ASSERT_LT(drive_command.torqueSetpoints[3], 12.5f); + + printf("torque 1: %.2f\n", drive_command.torqueSetpoints[0]); + printf("torque 2: %.2f\n", drive_command.torqueSetpoints[1]); + printf("torque 3: %.2f\n", drive_command.torqueSetpoints[2]); + printf("torque 4: %.2f\n", drive_command.torqueSetpoints[3]); +} + TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { SysClock clock = SysClock(); SysTick_s cur_tick;