Skip to content

Commit

Permalink
please see line 83 of test_torqueVectoring.cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
@shayana18
shayana18 committed Feb 2, 2025
1 parent 6b648c4 commit aac53da
Show file tree
Hide file tree
Showing 6 changed files with 152 additions and 95 deletions.
5 changes: 5 additions & 0 deletions can_bus/quadruna/VC/VC_enum.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,10 @@
"AHRS": 2,
"VELOCITY": 3,
"POSITION": 4
},
"TVInteralStates":{
"OFF": 0,
"TRACTION_CONTROL": 1,
"ACTIVE_DIFF": 2
}
}
10 changes: 10 additions & 0 deletions can_bus/quadruna/VC/VC_tx.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -794,5 +794,15 @@
"unit": "%"
}
}
},
"TorqueVectoringStateMachine":{
"msg_id":231,
"cycle_time": 100,
"description": "Torque vectoring internal state info",
"signals":{
"TVInternalState":{
"enum": "TVInteralStates"
}
}
}
}
130 changes: 71 additions & 59 deletions firmware/quadruna/VC/src/app/vehicle_dynamics/app_torqueVectoring.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
#include "app_utils.h"

#define MOTOR_NOT_SPINNING_SPEED_RPM 1000
#define TRACTION_CONTROL_LOWER_BOUND -5.0f
#define TRACTION_CONTROL_UPPER_BOUND 5.0f
#define TRACTION_CONTROL_LOWER_BOUND -10.0f
#define TRACTION_CONTROL_UPPER_BOUND 10.0f
#define PEDAL_NOT_PRESSED 0.0f
#define STEERING_ANG_HYSTERSIS 5.0f
static TimerChannel pid_timeout;
Expand All @@ -22,8 +22,8 @@ static TractionControl_Inputs traction_control_inputs;
static TractionControl_Outputs traction_control_outputs;
static Torque_to_Inverters torque_to_inverters;

static bool run_traction_control = false;
static torqueVectoringStates torqueVectoringInternalState= OFF;
static bool run_traction_control = true;
static TVInteralStates torqueVectoringInternalState= OFF;

// NOTE: Correction factor centered about 0.0f

Expand All @@ -47,51 +47,11 @@ static float right_motor_temp_C;
static float steering_angle_deg;


/* static functions*/

/* Event Handlers */

static torqueVectoringStates noPedalInput()
{
app_canTx_VC_LeftInverterTorqueCommand_set(0.0f);
app_canTx_VC_RightInverterTorqueCommand_set(0.0f);
return OFF;
}

static torqueVectoringStates runTractionControl(float power_limit)
{
if(torqueVectoringInternalState != TRACTION_CONTROL)
{
pid_traction_control.integral = 0; // WE DO NOT WANT TO KEEP RESETING ACCUMULATED ERROR TO 0 WHILE WE ARE CONTINUOUSLY IN TRACTION CONTORL
}
traction_control_inputs.motor_speed_left_rpm = motor_speed_left_rpm;
traction_control_inputs.motor_speed_right_rpm = motor_speed_right_rpm;
traction_control_inputs.torque_limit = app_torqueVectoring_powerToTorque(power_limit,
motor_speed_left_rpm,
motor_speed_right_rpm,
1,
1);
traction_control_inputs.torque_left_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
traction_control_inputs.torque_right_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
traction_control_inputs.wheel_speed_front_left_kph = wheel_speed_front_left_kph;
traction_control_inputs.wheel_speed_front_right_kph = wheel_speed_front_right_kph;
app_tractionControl_computeTorque(&traction_control_inputs, &traction_control_outputs);
return TRACTION_CONTROL;
}

static torqueVectoringStates runActiveDiff(float power_limit)
{
active_differential_inputs.power_max_kW = power_limit;
active_differential_inputs.motor_speed_left_rpm = motor_speed_left_rpm;
active_differential_inputs.motor_speed_right_rpm = motor_speed_right_rpm;
active_differential_inputs.wheel_angle_deg = steering_angle_deg * APPROX_STEERING_TO_WHEEL_ANGLE;
active_differential_inputs.requested_torque = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
app_activeDifferential_computeTorque(&active_differential_inputs, &active_differential_outputs);
app_canTx_VC_ActiveDiffTorqueLeft_set(active_differential_outputs.torque_left_Nm);
app_canTx_VC_ActiveDiffTorqueRight_set(active_differential_outputs.torque_right_Nm);
return ACTIVE_DIFF;

}
/* static function forward references*/
static TVInteralStates noPedalInput();
static TVInteralStates runTractionControl(float power_limit);
static TVInteralStates runActiveDiff(float power_limit);
static void app_torqueVectoring_stateMachine(float pedal_percentage);

/* public functions */
void app_torqueVectoring_init(void)
Expand All @@ -117,9 +77,9 @@ void app_torqueVectoring_run(float pedal_percentage)
}
app_timer_restart(&pid_timeout);

app_torqueVectoring_stateMachine(pedal_percentage);


app_torqueVectoring_stateMachine(pedal_percentage);
app_canTx_VC_TVInternalState_set(torqueVectoringInternalState);
torque_to_inverters.torque_left_final_Nm = traction_control_outputs.torque_left_Nm + active_differential_outputs.torque_left_Nm;
torque_to_inverters.torque_right_final_Nm = traction_control_outputs.torque_right_Nm + active_differential_outputs.torque_right_Nm;

Expand Down Expand Up @@ -160,7 +120,9 @@ void app_torqueVectoring_run(float pedal_percentage)
app_canTx_VC_PIDPowerEstimateIntegral_set(pid_power_correction.integral);
}

void app_torqueVectoring_stateMachine(float accelerator_pedal_percentage)
/* Static functions*/

static void app_torqueVectoring_stateMachine(float accelerator_pedal_percentage)
{
// Read data from CAN
// NOTE: Pedal percent CAN is in range 0.0-100.0%
Expand Down Expand Up @@ -192,7 +154,7 @@ void app_torqueVectoring_stateMachine(float accelerator_pedal_percentage)
traction_control_outputs.torque_left_Nm = 0.0;
traction_control_outputs.torque_right_Nm = 0.0;

if(accelerator_pedal_percent <= 0.0f && torqueVectoringInternalState != OFF)
if(accelerator_pedal_percent <= PEDAL_NOT_PRESSED && torqueVectoringInternalState != OFF)
{
torqueVectoringInternalState = noPedalInput();
return;
Expand All @@ -214,31 +176,35 @@ void app_torqueVectoring_stateMachine(float accelerator_pedal_percentage)
torqueVectoringInternalState = runActiveDiff(power_limit);
}
}

break;

case TRACTION_CONTROL:

if(!run_traction_control)
{
torqueVectoringInternalState = runActiveDiff(power_limit);
break;
}

if ((TRACTION_CONTROL_LOWER_BOUND - STEERING_ANG_HYSTERSIS) <= steering_angle_deg && steering_angle_deg <= (TRACTION_CONTROL_UPPER_BOUND + STEERING_ANG_HYSTERSIS))
// we do not nessecarily want hystersis to enter active diff from traction control as if we increase the window for to be activate for a larger angle window
// we would be further assuming that there are little lateral forces in play at a steering angle where they very much may be in play
if (TRACTION_CONTROL_LOWER_BOUND <= steering_angle_deg && steering_angle_deg <= TRACTION_CONTROL_UPPER_BOUND)
{
torqueVectoringInternalState = runActiveDiff(power_limit);
torqueVectoringInternalState = runTractionControl(power_limit);
}

else
{
torqueVectoringInternalState = runTractionControl(power_limit);
torqueVectoringInternalState = runActiveDiff(power_limit);
}

break;

case ACTIVE_DIFF:

if(run_traction_control)
{
// we implement hystersis here as we do not want to oscillate back into slip control by accident
if ((TRACTION_CONTROL_LOWER_BOUND + STEERING_ANG_HYSTERSIS) <= steering_angle_deg && steering_angle_deg <= (TRACTION_CONTROL_UPPER_BOUND - STEERING_ANG_HYSTERSIS))
{
torqueVectoringInternalState = runTractionControl(power_limit);
Expand All @@ -250,15 +216,61 @@ void app_torqueVectoring_stateMachine(float accelerator_pedal_percentage)
{
torqueVectoringInternalState = runActiveDiff(power_limit);
}

break;

default:
torqueVectoringInternalState = noPedalInput();
break;
}

return;
}

/* Event Handlers */

static TVInteralStates noPedalInput()
{
app_canTx_VC_LeftInverterTorqueCommand_set(0.0f);
app_canTx_VC_RightInverterTorqueCommand_set(0.0f);
return OFF;
}

static TVInteralStates runTractionControl(float power_limit)
{
if(torqueVectoringInternalState != TRACTION_CONTROL)
{
pid_traction_control.integral = 0; // WE DO NOT WANT TO KEEP RESETING ACCUMULATED ERROR TO 0 WHILE WE ARE CONTINUOUSLY IN TRACTION CONTORL
}
traction_control_inputs.motor_speed_left_rpm = motor_speed_left_rpm;
traction_control_inputs.motor_speed_right_rpm = motor_speed_right_rpm;
traction_control_inputs.torque_limit = app_torqueVectoring_powerToTorque(power_limit,
motor_speed_left_rpm,
motor_speed_right_rpm,
1,
1);
traction_control_inputs.torque_left_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
traction_control_inputs.torque_right_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
traction_control_inputs.wheel_speed_front_left_kph = wheel_speed_front_left_kph;
traction_control_inputs.wheel_speed_front_right_kph = wheel_speed_front_right_kph;
app_tractionControl_computeTorque(&traction_control_inputs, &traction_control_outputs);
return TRACTION_CONTROL;
}

static TVInteralStates runActiveDiff(float power_limit)
{
active_differential_inputs.power_max_kW = power_limit;
active_differential_inputs.motor_speed_left_rpm = motor_speed_left_rpm;
active_differential_inputs.motor_speed_right_rpm = motor_speed_right_rpm;
active_differential_inputs.wheel_angle_deg = steering_angle_deg * APPROX_STEERING_TO_WHEEL_ANGLE;
active_differential_inputs.requested_torque = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
app_activeDifferential_computeTorque(&active_differential_inputs, &active_differential_outputs);
app_canTx_VC_ActiveDiffTorqueLeft_set(active_differential_outputs.torque_left_Nm);
app_canTx_VC_ActiveDiffTorqueRight_set(active_differential_outputs.torque_right_Nm);
return ACTIVE_DIFF;

}



float app_torqueVectoring_powerToTorque(
float power_kW,
Expand Down
7 changes: 0 additions & 7 deletions firmware/quadruna/VC/src/app/vehicle_dynamics/app_torqueVectoring.h
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,7 @@
#pragma once

typedef enum{
OFF,
TRACTION_CONTROL,
ACTIVE_DIFF
}torqueVectoringStates;

void app_torqueVectoring_init(void);
void app_torqueVectoring_run(float accelerator_pedal_percentage);
void app_torqueVectoring_stateMachine(float pedal_percentage);
float app_torqueVectoring_powerToTorque(
float power_kW,
float left_motor_speed_rpm,
Expand Down
89 changes: 63 additions & 26 deletions firmware/quadruna/VC/test/vehicle_dynamics/test_torqueVectoring.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@
*/

#include <gtest/gtest.h>
#include <stdio.h>
#include "test_vcBaseStateMachineTest.h"

extern "C"
{
Expand All @@ -22,7 +24,7 @@ extern "C"
#include "app_canTx.h"
}

class TorqueVectoringTest : public testing::Test
class TorqueVectoringTest : public VcBaseStateMachineTest
{
protected:
void SetUp() override
Expand All @@ -33,29 +35,16 @@ class TorqueVectoringTest : public testing::Test
}
};

// TEST_F(TorqueVectoringTest, torques_are_zero_when_power_limit_is_zero)
// {
// app_canRx_FSM_PappsMappedPedalPercentage_update(100.0);
// app_canRx_FSM_LeftWheelSpeed_update(50.0);
// app_canRx_FSM_RightWheelSpeed_update(50.0);
// app_canRx_INVL_MotorSpeed_update(135);
// app_canRx_INVR_MotorSpeed_update(135);
// app_canRx_BMS_TractiveSystemCurrent_update(100);
// app_canRx_BMS_TractiveSystemVoltage_update(390);
// app_canRx_INVL_MotorTemperature_update(50);
// app_canRx_INVR_MotorTemperature_update(50);
// app_canRx_BMS_AvailablePower_update(0);
// app_canRx_FSM_Warning_SteeringAngleOCSC_update(30);
// app_torqueVectoring_run();
// float expected_torque_left_nM = 0.0;
// float expected_torque_right_nM = 0.0;
// float actual_torque_left_nM = app_canTx_VC_LeftInverterTorqueCommand_get();
// float actual_torque_right_nM = app_canTx_VC_RightInverterTorqueCommand_get();
// ASSERT_FLOAT_EQ(expected_torque_left_nM, actual_torque_left_nM);
// ASSERT_FLOAT_EQ(expected_torque_right_nM, actual_torque_right_nM);
// }
TEST_F(TorqueVectoringTest, torques_are_zero_when_pedal_is_not_pressed)
TEST_F(TorqueVectoringTest, stateMachineTest)
{
TearDown();
SetUp();
app_canRx_CRIT_TorqueVecSwitch_update(SWITCH_ON);
SetStateToDrive();

EXPECT_EQ(VC_DRIVE_STATE, app_canTx_VC_State_get());
ASSERT_TRUE(app_canTx_VC_TorqueVectoringEnabled_get());

app_canRx_FSM_LeftWheelSpeed_update(50.0);
app_canRx_FSM_RightWheelSpeed_update(50.0);
app_canRx_INVL_MotorSpeed_update(135);
Expand All @@ -65,16 +54,64 @@ TEST_F(TorqueVectoringTest, torques_are_zero_when_pedal_is_not_pressed)
app_canRx_INVL_MotorTemperature_update(50);
app_canRx_INVR_MotorTemperature_update(50);
app_canRx_BMS_AvailablePower_update(50);
app_canRx_FSM_Warning_SteeringAngleOCSC_update(30);
app_torqueVectoring_run(0.0f);
app_canRx_FSM_SteeringAngle_update(30);
app_canRx_FSM_PappsMappedPedalPercentage_update(0.0);
app_canRx_FSM_SappsMappedPedalPercentage_update(0.0);

float expected_torque_left_nM = 0.0;
float expected_torque_right_nM = 0.0;
float actual_torque_left_nM = app_canTx_VC_LeftInverterTorqueCommand_get();

// Torques should be 0 for no pedal input
float actual_torque_left_nM = app_canTx_VC_LeftInverterTorqueCommand_get();
float actual_torque_right_nM = app_canTx_VC_RightInverterTorqueCommand_get();
ASSERT_FLOAT_EQ(expected_torque_left_nM, actual_torque_left_nM);
ASSERT_FLOAT_EQ(expected_torque_right_nM, actual_torque_right_nM);

// Interal State should be OFF regardless of steering angle if pedal input is 0
// 0 = Off, 1 = Traction Control, 2 = Active Differential
ASSERT_TRUE(app_canTx_VC_TorqueVectoringEnabled_get());
ASSERT_EQ(app_canTx_VC_TVInternalState_get(), 0);
LetTimePass(10);


// pedal activated and steering above slip thresh
ASSERT_TRUE(app_canTx_VC_TorqueVectoringEnabled_get());
app_canRx_FSM_SteeringAngle_update(30);
app_torqueVectoring_run(0.4f);
ASSERT_EQ(app_canTx_VC_TVInternalState_get(), 2);
LetTimePass(10);
// ASSERT_EQ(app_canTx_VC_TVInternalState_get(), 2); -- WHY IS STATE RESETTING TO OFF????????



// the range for slip control is [-10,10] without any hystersis, here if we come from off state we shouldnt enter traction control in
// [-10,10]
ASSERT_TRUE(app_canTx_VC_TorqueVectoringEnabled_get());

// // should stay in active diff
// app_canRx_FSM_SteeringAngle_update(30);
// app_torqueVectoring_run(0.6f);
// ASSERT_EQ(app_canTx_VC_TVInternalState_get(), 2);
// LetTimePass(10);

// // should stay in active diff at 10 because of hystersis buffer
// app_canRx_FSM_SteeringAngle_update(10);
// app_torqueVectoring_run(0.6f);
// LetTimePass(10);



// // testing edge case for traction control + hystersis
// app_canRx_FSM_SteeringAngle_update(5);
// app_torqueVectoring_run(1.0f);
// // ASSERT_EQ(app_canTx_VC_TVInternalState_get(), 1);




}


// TEST_F(TorqueVectoringTest, torques_are_zero_when_wheels_are_not_moving)
// {
// app_canRx_FSM_PappsMappedPedalPercentage_update(100.0);
Expand Down
6 changes: 3 additions & 3 deletions firmware/quadruna/VC/test/vehicle_dynamics/test_tractionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,11 @@ TEST(TractionControlTest, TractionControl_init)
PID_Config tc_pid_config = { 0.0, 0.0, 0.0, 0.0, 10.0 };
PID tc_pid;
app_pid_init(&tc_pid, &tc_pid_config);
TractionControl_Inputs test_inputs = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, &tc_pid };
TractionControl_Inputs test_inputs = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,&tc_pid };
TractionControl_Outputs test_outputs = { 0.0, 0.0 };
app_tractionControl_computeTorque(&test_inputs, &test_outputs);
ASSERT_NEAR(test_outputs.torque_left_final_Nm, 0.0, 0.000001);
ASSERT_NEAR(test_outputs.torque_right_final_Nm, 0.0, 0.000001);
ASSERT_NEAR(test_outputs.torque_left_Nm, 0.0, 0.000001);
ASSERT_NEAR(test_outputs.torque_right_Nm, 0.0, 0.000001);
}

TEST(TractionControlTest, TractionControl_slip)
Expand Down

0 comments on commit aac53da

Please sign in to comment.