From 47265b3358f3857e08ea2bb831ed24c7a3a029f0 Mon Sep 17 00:00:00 2001 From: walkermburns Date: Wed, 10 Apr 2024 21:33:42 -0400 Subject: [PATCH 01/10] Finished writing initial torque and power limit --- lib/systems/include/PhysicalParameters.h | 3 ++ lib/systems/include/TorqueControllerMux.h | 11 ++++++- lib/systems/include/TorqueControllers.h | 20 +++---------- lib/systems/src/TorqueControllerMux.cpp | 36 +++++++++++++++++++++++ lib/systems/src/TorqueControllers.cpp | 6 ---- 5 files changed, 53 insertions(+), 23 deletions(-) diff --git a/lib/systems/include/PhysicalParameters.h b/lib/systems/include/PhysicalParameters.h index c090d6846..0b8532fc8 100644 --- a/lib/systems/include/PhysicalParameters.h +++ b/lib/systems/include/PhysicalParameters.h @@ -6,4 +6,7 @@ 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 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..da86a08e5 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; @@ -94,14 +96,21 @@ class TorqueControllerMux { return drivetrainCommand_; }; + const TorqueLimit_e &getTorqueLimit() { return torqueLimit_; }; + const float getMaxTorque() { return torqueLimitMap_[torqueLimit_]; } + + void applyPowerLimit(DrivetrainCommand_s* command); + + void applyTorqueLimit(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 d7e690de9..c2db3168f 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 = 63.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: }; diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 21faac2e7..84225d21d 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -85,5 +85,41 @@ void TorqueControllerMux::tick( } drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; + + applyPowerLimit(&drivetrainCommand_); + applyTorqueLimit(&drivetrainCommand_); + + } +} + +void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command) +{ + float net_torque_mag = 0; + + for (int i = 0; i < NUM_MOTORS; i++) { + net_torque_mag += abs(command->torqueSetpoints[i]); + } + + for (int i = 0; i < NUM_MOTORS; i++) { + float power_per_corner = (command->torqueSetpoints[i] / net_torque_mag) * MAX_POWER_LIMIT * 1000; + command->torqueSetpoints[i] = power_per_corner / (command->speeds_rpm[i] * RPM_TO_RAD_PER_SECOND); } + +} + +void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s* command) +{ + float max_torque = getMaxTorque(); + float avg_torque; + + for (int i = 0; i < NUM_MOTORS; i++) { + avg_torque += abs(command->torqueSetpoints[i]); + } + avg_torque /= NUM_MOTORS; + + if (avg_torque > max_torque) { + float scale = avg_torque / max_torque; + for (int i = 0; i < NUM_MOTORS; i++) { command->torqueSetpoints[i] /= scale; } + } + } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index d320aeb09..ec1d87878 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); } } @@ -145,9 +142,6 @@ void TorqueControllerLoadCellVectoring::tick( writeout_.command.torqueSetpoints[RL] = torqueRequest; writeout_.command.torqueSetpoints[RR] = torqueRequest; } - - // Apply the torque limit - TCPosTorqueLimit(writeout_.command, torqueLimit); } else { From fee802e0ede7ff94fc3a299a2e5b06e8f9464288 Mon Sep 17 00:00:00 2001 From: walkermburns Date: Thu, 11 Apr 2024 17:13:04 -0400 Subject: [PATCH 02/10] Finished writing and implementing torque, power, and neg speed limits. current tests passing --- lib/systems/include/TorqueControllerMux.h | 2 ++ lib/systems/src/TorqueControllerMux.cpp | 43 +++++++++++++++++++++-- 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index da86a08e5..00e328e6d 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -111,6 +111,8 @@ class TorqueControllerMux void applyTorqueLimit(DrivetrainCommand_s* command); + void applyPosSpeedLimit(DrivetrainCommand_s* command); + TorqueControllerBase* activeController() { // check to make sure that there is actually a controller diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 84225d21d..27c32516c 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -88,38 +88,77 @@ void TorqueControllerMux::tick( applyPowerLimit(&drivetrainCommand_); applyTorqueLimit(&drivetrainCommand_); + applyPosSpeedLimit(&drivetrainCommand_); } } +/* + 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) { float net_torque_mag = 0; for (int i = 0; i < NUM_MOTORS; i++) { + // get the total magnitude of torque from all 4 wheels + #ifdef ARDUINO_TEENSY41 net_torque_mag += abs(command->torqueSetpoints[i]); + #else + net_torque_mag += std::abs(command->torqueSetpoints[i]); + #endif } for (int i = 0; i < NUM_MOTORS; i++) { - float power_per_corner = (command->torqueSetpoints[i] / net_torque_mag) * MAX_POWER_LIMIT * 1000; + // 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) * 1000; + // power / omega (motor rad/s) to get torque per wheel command->torqueSetpoints[i] = power_per_corner / (command->speeds_rpm[i] * RPM_TO_RAD_PER_SECOND); + + // isn't this just always distributing max power to all wheels? } } +/* + 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; + 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 From 4b5d4d04ba5237252b62e295e8df8c59bcb9fc82 Mon Sep 17 00:00:00 2001 From: walkermburns Date: Thu, 11 Apr 2024 19:04:46 -0400 Subject: [PATCH 03/10] Began writing tests for power limit --- lib/systems/include/TorqueControllerMux.h | 1 + lib/systems/include/TorqueControllers.h | 2 +- lib/systems/src/TorqueControllerMux.cpp | 29 ++++++++++++------- .../test_systems/torque_controller_mux_test.h | 29 +++++++++++++++++++ 4 files changed, 50 insertions(+), 11 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 00e328e6d..2841487d9 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -92,6 +92,7 @@ class TorqueControllerMux bool dashboardTorqueModeButtonPressed, const vector_nav &vn_data, float wheel_angle_rad); + const DrivetrainCommand_s &getDrivetrainCommand() { return drivetrainCommand_; diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index c2db3168f..e4b52e3ad 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -17,7 +17,7 @@ /* CONTROLLER CONSTANTS */ -const float MAX_POWER_LIMIT = 63.0; // max mechanical power limit in KW +const float MAX_POWER_LIMIT = 63000.0; // max mechanical power limit in KW /* MOTOR CONSTANTS */ diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 27c32516c..9d5cc12ba 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -101,25 +101,34 @@ void TorqueControllerMux::tick( void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command) { float net_torque_mag = 0; + float net_wheelspeed = 0; for (int i = 0; i < NUM_MOTORS; i++) { // get the total magnitude of torque from all 4 wheels - #ifdef ARDUINO_TEENSY41 + #ifdef ARDUINO_TEENSY41 // screw arduino.h macros net_torque_mag += abs(command->torqueSetpoints[i]); + net_wheelspeed += abs(command->speeds_rpm[i]); #else net_torque_mag += std::abs(command->torqueSetpoints[i]); + net_wheelspeed += std::abs(command->speeds_rpm[i]); #endif } + + // get current total mechanical power + float cur_power = net_torque_mag * (net_wheelspeed * RPM_TO_RAD_PER_SECOND); - 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) * 1000; - // power / omega (motor rad/s) to get torque per wheel - command->torqueSetpoints[i] = power_per_corner / (command->speeds_rpm[i] * RPM_TO_RAD_PER_SECOND); - - // isn't this just always distributing max power to all wheels? + // only evaluate power limit if current power exceeds it + if (cur_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 / (command->speeds_rpm[i] * RPM_TO_RAD_PER_SECOND); + + // isn't this just always distributing max power to all wheels? + } } } diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index fbd85bcf8..7b47caccb 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -287,6 +287,35 @@ TEST(TorqueControllerMuxTesting, test_speed_delta_prevents_mode_change) } } +TEST(TorqueControllerMuxTesting, test_power_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; + } + + mux.applyPowerLimit(&drive_command); + + for (int i = 0; i < 4; i++) { + ASSERT_EQ(drive_command.speeds_rpm[i], 500.0f); + ASSERT_EQ(drive_command.torqueSetpoints[i], 10.0f); + } + + for (int i = 0; i < 4; i++) { + drive_command.speeds_rpm[i] = 20000.0f; + drive_command.torqueSetpoints[i] = 21.0f; + } + + mux.applyPowerLimit(&drive_command); + + for (int i = 0; i < 4; i++) { + ASSERT_LT(drive_command.torqueSetpoints[i], 7.6); // hardcoded value based on online calculator + } + +} + TEST(TorqueControllerMuxTesting, test_simple_launch_controller) { SysClock clock = SysClock(); SysTick_s cur_tick; From 0a6e0225f85dc5feb14da762fd98ed6c0d8db0bb Mon Sep 17 00:00:00 2001 From: walkermburns Date: Thu, 11 Apr 2024 19:28:07 -0400 Subject: [PATCH 04/10] Some basic tests for torque and power limit --- lib/systems/src/TorqueControllerMux.cpp | 9 ++++- .../test_systems/torque_controller_mux_test.h | 37 +++++++++++++++++++ 2 files changed, 45 insertions(+), 1 deletion(-) diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 9d5cc12ba..70f22e60a 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -2,6 +2,8 @@ #include "Utility.h" #include "PhysicalParameters.h" +#include + void TorqueControllerMux::tick( const SysTick_s &tick, const DrivetrainDynamicReport_s &drivetrainData, @@ -154,12 +156,17 @@ void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s* command) } avg_torque /= NUM_MOTORS; + printf("max torque: %.2f\n", max_torque); + printf("avg torque: %.2f\n", avg_torque); // 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; } + printf("scale: %.2f\n", scale); + for (int i = 0; i < NUM_MOTORS; i++) { + command->torqueSetpoints[i] /= scale; + } } } diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index 7b47caccb..b75d0a639 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -316,6 +316,43 @@ TEST(TorqueControllerMuxTesting, test_power_limit) { } +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; From 1bf1702ae0cdbd8b0da8ee7c9a44cc772cbaf153 Mon Sep 17 00:00:00 2001 From: walkermburns Date: Thu, 11 Apr 2024 20:37:47 -0400 Subject: [PATCH 05/10] Fixed some power limiting concerns --- lib/systems/include/TorqueControllerMux.h | 2 +- lib/systems/src/TorqueControllerMux.cpp | 17 ++++++----------- test/test_systems/torque_controller_mux_test.h | 15 ++++++++++----- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 2841487d9..47b8fbc34 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -108,7 +108,7 @@ class TorqueControllerMux return torqueLimitMap_[torqueLimit_]; } - void applyPowerLimit(DrivetrainCommand_s* command); + void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); void applyTorqueLimit(DrivetrainCommand_s* command); diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 70f22e60a..8e8bb482f 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -2,8 +2,6 @@ #include "Utility.h" #include "PhysicalParameters.h" -#include - void TorqueControllerMux::tick( const SysTick_s &tick, const DrivetrainDynamicReport_s &drivetrainData, @@ -88,7 +86,7 @@ void TorqueControllerMux::tick( drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; - applyPowerLimit(&drivetrainCommand_); + applyPowerLimit(&drivetrainCommand_, &drivetrainData); applyTorqueLimit(&drivetrainCommand_); applyPosSpeedLimit(&drivetrainCommand_); @@ -100,7 +98,7 @@ void TorqueControllerMux::tick( exceeds the preset mechanical power limit. Scales all wheels down to preserve functionality of torque controllers */ -void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command) +void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) { float net_torque_mag = 0; float net_wheelspeed = 0; @@ -109,10 +107,10 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command) // 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_wheelspeed += abs(command->speeds_rpm[i]); + net_wheelspeed += abs(drivetrain->measuredSpeeds[i]); #else net_torque_mag += std::abs(command->torqueSetpoints[i]); - net_wheelspeed += std::abs(command->speeds_rpm[i]); + net_wheelspeed += std::abs(drivetrain->measuredSpeeds[i]); #endif } @@ -127,7 +125,7 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command) // 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 / (command->speeds_rpm[i] * RPM_TO_RAD_PER_SECOND); + command->torqueSetpoints[i] = power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND); // isn't this just always distributing max power to all wheels? } @@ -155,15 +153,12 @@ void TorqueControllerMux::applyTorqueLimit(DrivetrainCommand_s* command) #endif } avg_torque /= NUM_MOTORS; - - printf("max torque: %.2f\n", max_torque); - printf("avg torque: %.2f\n", avg_torque); + // 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 - printf("scale: %.2f\n", scale); for (int i = 0; i < NUM_MOTORS; i++) { command->torqueSetpoints[i] /= scale; } diff --git a/test/test_systems/torque_controller_mux_test.h b/test/test_systems/torque_controller_mux_test.h index b75d0a639..5283e3eec 100644 --- a/test/test_systems/torque_controller_mux_test.h +++ b/test/test_systems/torque_controller_mux_test.h @@ -291,24 +291,29 @@ TEST(TorqueControllerMuxTesting, test_power_limit) { TorqueControllerMux mux = TorqueControllerMux(); DrivetrainCommand_s drive_command; + DrivetrainDynamicReport_s edit; + for (int i = 0; i < 4; i++) { - drive_command.speeds_rpm[i] = 500.0f; + edit.measuredSpeeds[i] = 500.0f; drive_command.torqueSetpoints[i] = 10.0f; } - mux.applyPowerLimit(&drive_command); + const DrivetrainDynamicReport_s drivetrain1 = edit; + + mux.applyPowerLimit(&drive_command, &drivetrain1); for (int i = 0; i < 4; i++) { - ASSERT_EQ(drive_command.speeds_rpm[i], 500.0f); ASSERT_EQ(drive_command.torqueSetpoints[i], 10.0f); } for (int i = 0; i < 4; i++) { - drive_command.speeds_rpm[i] = 20000.0f; + edit.measuredSpeeds[i] = 20000.0f; drive_command.torqueSetpoints[i] = 21.0f; } - mux.applyPowerLimit(&drive_command); + 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 From 513626d05a37628547df8a8c956353cad8e11a4b Mon Sep 17 00:00:00 2001 From: walkermburns Date: Tue, 16 Apr 2024 18:06:35 -0400 Subject: [PATCH 06/10] Initial regen limit added --- lib/systems/include/TorqueControllerMux.h | 5 ++++- lib/systems/src/TorqueControllerMux.cpp | 25 ++++++++++++++++++++++- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 47b8fbc34..294eb44f1 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -108,12 +108,15 @@ class TorqueControllerMux return torqueLimitMap_[torqueLimit_]; } - void applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain); + 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/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 8e8bb482f..8381abf08 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -86,13 +86,36 @@ void TorqueControllerMux::tick( drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; - applyPowerLimit(&drivetrainCommand_, &drivetrainData); + // 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 5m/s on all wheels. +*/ +void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) +{ + float max_wheelspeed = 0; + bool regen = false; + + for (int i = 0; i < NUM_MOTORS; i++) { + max_wheelspeed = std::max(max_wheelspeed, drivetrain->measuredSpeeds[i]); + regen = (command->speeds_rpm[i] == 0); + } + + if ((max_wheelspeed * RPM_TO_METERS_PER_SECOND) < 5 && regen) { + for (int i = 0; i < NUM_MOTORS; i++) { + command->torqueSetpoints[i] = 0; + } + } +} + /* Apply power limit such that the mechanical power of all wheels never exceeds the preset mechanical power limit. Scales all wheels down to From dce60c6666428cd38510ff77adc62c3b4bc6052a Mon Sep 17 00:00:00 2001 From: Dopp-IO <42275088+Dopp-IO@users.noreply.github.com> Date: Tue, 16 Apr 2024 20:17:29 -0400 Subject: [PATCH 07/10] fix regen balance for load cell TC --- lib/systems/include/TorqueControllerMux.h | 2 +- lib/systems/include/TorqueControllers.h | 21 ++++++++++++++------- lib/systems/src/TorqueControllers.cpp | 8 ++++---- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 294eb44f1..8f3410a0a 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -74,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)]) {} diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index 389c35bc8..151c95705 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -149,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), @@ -175,6 +174,8 @@ class TorqueControllerLoadCellVectoring : public TorqueController Date: Tue, 16 Apr 2024 20:26:01 -0400 Subject: [PATCH 08/10] adding &= for regen limit --- lib/systems/src/TorqueControllerMux.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 8381abf08..6dd4d1ed6 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -106,7 +106,7 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const Dr for (int i = 0; i < NUM_MOTORS; i++) { max_wheelspeed = std::max(max_wheelspeed, drivetrain->measuredSpeeds[i]); - regen = (command->speeds_rpm[i] == 0); + regen &= (command->speeds_rpm[i] == 0); } if ((max_wheelspeed * RPM_TO_METERS_PER_SECOND) < 5 && regen) { From 0fd8b1bbef5352cdeacd65a888bd2ce64cb37f74 Mon Sep 17 00:00:00 2001 From: Dopp-IO <42275088+Dopp-IO@users.noreply.github.com> Date: Tue, 16 Apr 2024 20:50:46 -0400 Subject: [PATCH 09/10] fix/improve 5kph regen limit --- lib/systems/include/PhysicalParameters.h | 9 +++++---- lib/systems/src/TorqueControllerMux.cpp | 22 +++++++++++++++------- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/lib/systems/include/PhysicalParameters.h b/lib/systems/include/PhysicalParameters.h index 0b8532fc8..7f971575a 100644 --- a/lib/systems/include/PhysicalParameters.h +++ b/lib/systems/include/PhysicalParameters.h @@ -1,10 +1,11 @@ #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; diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 6dd4d1ed6..ad86f6cbd 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -97,21 +97,29 @@ void TorqueControllerMux::tick( /* Apply limit to make sure that regenerative braking is not applied when - wheelspeed is below 5m/s on all wheels. + wheelspeed is below 5kph on all wheels. */ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) { - float max_wheelspeed = 0; - bool regen = false; + 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++) { - max_wheelspeed = std::max(max_wheelspeed, drivetrain->measuredSpeeds[i]); - regen &= (command->speeds_rpm[i] == 0); + maxWheelSpeed = std::max(maxWheelSpeed, drivetrain->measuredSpeeds[i] * RPM_TO_KILOMETERS_PER_HOUR); + allWheelsRegen &= (command->speeds_rpm[i] < drivetrain->measuredSpeeds[i]); } - if ((max_wheelspeed * RPM_TO_METERS_PER_SECOND) < 5 && regen) { + // 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] = 0; + command->torqueSetpoints[i] *= torqueScaleDown; } } } From 799a9c1b30ca130e2af38cf12580ceb68d98bbfd Mon Sep 17 00:00:00 2001 From: walkermburns Date: Wed, 17 Apr 2024 09:35:47 -0400 Subject: [PATCH 10/10] Update power limit to use summed power calculations of individual wheels --- lib/systems/src/TorqueControllerMux.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index ad86f6cbd..9b6706e2e 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -132,24 +132,24 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s* command, const Dr void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const DrivetrainDynamicReport_s* drivetrain) { float net_torque_mag = 0; - float net_wheelspeed = 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_wheelspeed += abs(drivetrain->measuredSpeeds[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]); - net_wheelspeed += std::abs(drivetrain->measuredSpeeds[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 } - - // get current total mechanical power - float cur_power = net_torque_mag * (net_wheelspeed * RPM_TO_RAD_PER_SECOND); // only evaluate power limit if current power exceeds it - if (cur_power > (MAX_POWER_LIMIT)) { + 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; @@ -157,8 +157,6 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s* command, const Dr 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); - - // isn't this just always distributing max power to all wheels? } }