Merge pull request #73 from hytech-racing/feature/power_and_torque_limit

Power, torque, and negative speedpoint limit

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__ */

lib/systems/include/TorqueControllerMux.h

@@ -2,6 +2,8 @@
 #define __TORQUECTRLMUX_H__
 
 #include <unordered_map>
+#include <cmath>
+
 #include "TorqueControllers.h"
 #include "DrivetrainSystem.h"
 #include "PedalsSystem.h"
@@ -29,7 +31,7 @@ class TorqueControllerMux
     std::unordered_map<TorqueLimit_e, float> 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<int>(TorqueController_e::TC_NO_CONTROLLER)])
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale)
-    , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)])
+    , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)], 1.0, simpleTCRegenTorqueScale)
     , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)])
     , torqueControllerSlipLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SLIP_LAUNCH)])
     , torqueControllerPIDTV_(controllerOutputs_[static_cast<int>(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

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<TC_SAFE_MODE>
     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<TC_LOAD_CELL_V
     TorqueControllerOutput_s &writeout_;
     float frontTorqueScale_ = 1.0;
     float rearTorqueScale_ = 1.0;
+    float frontRegenTorqueScale_ = 1.0;
+    float rearRegenTorqueScale_ = 1.0;
     /*
     FIR filter designed with
     http://t-filter.appspot.com
@@ -220,15 +209,21 @@ class TorqueControllerLoadCellVectoring : public TorqueController<TC_LOAD_CELL_V
     bool ready_ = false;
 
 public:
-    TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale)
+    /// @brief load cell 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 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
+    TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale)
         : writeout_(writeout),
           frontTorqueScale_(2.0 - rearTorqueScale),
-          rearTorqueScale_(rearTorqueScale)
+          rearTorqueScale_(rearTorqueScale),
+          frontRegenTorqueScale_(2.0 - regenTorqueScale),
+          rearRegenTorqueScale_(regenTorqueScale)
     {
         writeout_.command = TC_COMMAND_NO_TORQUE;
         writeout_.ready = false;
     }
-    TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0) {}
+    TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0, 1.0) {}
 
     void tick(
         const SysTick_s &tick,

lib/systems/src/TorqueControllerMux.cpp

@@ -85,5 +85,120 @@ 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);
+        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]);
     }
 }

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
         {

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;