Skip to content

Commit

Permalink
Merge pull request #73 from hytech-racing/feature/power_and_torque_limit
Browse files Browse the repository at this point in the history
Power, torque, and negative speedpoint limit
  • Loading branch information
walkermburns authored Apr 19, 2024
2 parents 047a46e + 799a9c1 commit d12cc96
Showing 6 changed files with 233 additions and 39 deletions.
12 changes: 8 additions & 4 deletions lib/systems/include/PhysicalParameters.h
Original file line number Diff line number Diff line change
@@ -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__ */
19 changes: 17 additions & 2 deletions lib/systems/include/TorqueControllerMux.h
Original file line number Diff line number Diff line change
@@ -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
41 changes: 18 additions & 23 deletions lib/systems/include/TorqueControllers.h
Original file line number Diff line number Diff line change
@@ -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,
115 changes: 115 additions & 0 deletions lib/systems/src/TorqueControllerMux.cpp
Original file line number Diff line number Diff line change
@@ -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]);
}
}
14 changes: 4 additions & 10 deletions lib/systems/src/TorqueControllers.cpp
Original file line number Diff line number Diff line change
@@ -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
{
71 changes: 71 additions & 0 deletions test/test_systems/torque_controller_mux_test.h
Original file line number Diff line number Diff line change
@@ -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;

0 comments on commit d12cc96

Please sign in to comment.