Skip to content

Commit

Permalink
initial run at architecture re-design
Browse files Browse the repository at this point in the history
  • Loading branch information
@shayana18
shayana18 committed Jan 21, 2025
1 parent 272982c commit 526083c
Show file tree
Hide file tree
Showing 4 changed files with 98 additions and 32 deletions.
88 changes: 63 additions & 25 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,13 +9,16 @@
#include "app_utils.h"

#define MOTOR_NOT_SPINNING_SPEED_RPM 1000
#define TRACTION_CONTROL_LOWER_BOUND -10.0f
#define TRACTION_CONTROL_UPPER_BOUND 10.0f
static TimerChannel pid_timeout;

static PowerLimiting_Inputs power_limiting_inputs = { .power_limit_kW = POWER_LIMIT_CAR_kW };
static ActiveDifferential_Inputs active_differential_inputs;
static ActiveDifferential_Outputs active_differential_outputs;
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;

Expand Down Expand Up @@ -96,57 +99,80 @@ void app_torqueVectoring_handleAcceleration(void)
// Power limit correction
float power_limit = estimated_power_limit * (1.0f + pid_power_correction_factor);

// Active Differential
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);
// initializing active diff outputs
active_differential_outputs.torque_left_Nm = 0.0;
active_differential_outputs.torque_right_Nm = 0.0;

// initializing active diff outputs
traction_control_outputs.torque_left_Nm = 0.0;
traction_control_outputs.torque_right_Nm = 0.0;



// new control architecture
if(TRACTION_CONTROL_LOWER_BOUND <= steering_angle_deg && steering_angle_deg <= TRACTION_CONTROL_UPPER_BOUND)
{

float torque_lim = app_torqueVectoring_powerToTorque(power_limit,
motor_speed_left_rpm,
motor_speed_right_rpm,
1,
1);

/**
* TRACTION CONTROL NOT TESTED ON CAR YET
*/
// Traction Control

if (run_traction_control)
{
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_left_Nm = active_differential_outputs.torque_left_Nm;
traction_control_inputs.torque_right_Nm = active_differential_outputs.torque_right_Nm;
traction_control_inputs.torque_limit = torque_lim;
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);
}
}

else
{
// Active Differential
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);
}

// Inverter Torque Request
float torque_left_final_Nm;
float torque_right_final_Nm;
if (run_traction_control)
{
torque_left_final_Nm = traction_control_outputs.torque_left_final_Nm;
torque_right_final_Nm = traction_control_outputs.torque_right_final_Nm;
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;
}
else
{
torque_left_final_Nm = active_differential_outputs.torque_left_Nm;
torque_right_final_Nm = active_differential_outputs.torque_right_Nm;
torque_to_inverters.torque_left_final_Nm = active_differential_outputs.torque_left_Nm;
torque_to_inverters.torque_right_final_Nm = active_differential_outputs.torque_right_Nm;
}

// Limit asymptotic torques at zero speed
if (motor_speed_left_rpm < MOTOR_NOT_SPINNING_SPEED_RPM || motor_speed_right_rpm < MOTOR_NOT_SPINNING_SPEED_RPM)
{
torque_left_final_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
torque_right_final_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
torque_to_inverters.torque_left_final_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
torque_to_inverters.torque_right_final_Nm = accelerator_pedal_percent * MAX_TORQUE_REQUEST_NM;
}

// CLAMPS for safety only - should never exceed torque limit
torque_left_final_Nm = CLAMP(torque_left_final_Nm, 0, MAX_TORQUE_REQUEST_NM);
torque_right_final_Nm = CLAMP(torque_right_final_Nm, 0, MAX_TORQUE_REQUEST_NM);
app_canTx_VC_LeftInverterTorqueCommand_set(torque_left_final_Nm);
app_canTx_VC_RightInverterTorqueCommand_set(torque_right_final_Nm);
torque_to_inverters.torque_left_final_Nm = CLAMP(torque_to_inverters.torque_left_final_Nm, 0, MAX_TORQUE_REQUEST_NM);
torque_to_inverters.torque_right_final_Nm = CLAMP(torque_to_inverters.torque_right_final_Nm, 0, MAX_TORQUE_REQUEST_NM);
app_canTx_VC_LeftInverterTorqueCommand_set(torque_to_inverters.torque_left_final_Nm);
app_canTx_VC_RightInverterTorqueCommand_set(torque_to_inverters.torque_right_final_Nm);

// Calculate power correction PID
float power_consumed_measured = battery_voltage * current_consumption;
Expand All @@ -157,7 +183,8 @@ void app_torqueVectoring_handleAcceleration(void)
// POWER_TO_TORQUE_CONVERSION_FACTOR;

float power_consumed_ideal =
(motor_speed_left_rpm * torque_left_final_Nm + motor_speed_right_rpm * torque_right_final_Nm) /
(motor_speed_left_rpm * torque_to_inverters.torque_left_final_Nm +
motor_speed_right_rpm * torque_to_inverters.torque_right_final_Nm) /
POWER_TO_TORQUE_CONVERSION_FACTOR;
float power_consumed_estimate = power_consumed_ideal / (1.0f + pid_power_correction_factor);
pid_power_correction_factor -=
Expand All @@ -170,3 +197,14 @@ void app_torqueVectoring_handleAcceleration(void)
app_canTx_VC_PIDPowerEstimateDerivative_set(pid_power_correction.derivative);
app_canTx_VC_PIDPowerEstimateIntegral_set(pid_power_correction.integral);
}

float app_torqueVectoring_powerToTorque(
float power_kW,
float left_motor_speed_rpm,
float right_motor_speed_rpm,
float cl,
float cr)
{
return (POWER_TO_TORQUE_CONVERSION_FACTOR * power_kW) /
(left_motor_speed_rpm * cl + right_motor_speed_rpm * cr + SMALL_EPSILON);
}
6 changes: 6 additions & 0 deletions firmware/quadruna/VC/src/app/vehicle_dynamics/app_torqueVectoring.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,3 +3,9 @@
void app_torqueVectoring_init(void);
void app_torqueVectoring_run(float accelerator_pedal_percentage);
void app_torqueVectoring_handleAcceleration(void);
float app_torqueVectoring_powerToTorque(
float power_kW,
float left_motor_speed_rpm,
float right_motor_speed_rpm,
float cl,
float cr);
25 changes: 20 additions & 5 deletions firmware/quadruna/VC/src/app/vehicle_dynamics/app_tractionControl.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,27 @@ void app_tractionControl_computeTorque(TractionControl_Inputs *inputs, TractionC
float slip_ratio_left = app_tractionControl_computeSlip(inputs->motor_speed_left_rpm, wheel_speed_front_left_rpm);
float slip_ratio_right =
app_tractionControl_computeSlip(inputs->motor_speed_right_rpm, wheel_speed_front_right_rpm);

float error_slip_left = (float)fabs(SLIP_RATIO_IDEAL - slip_ratio_left);
float error_slip_right = (float)fabs(SLIP_RATIO_IDEAL - slip_ratio_right);

float slip_ratio_max = fmaxf(slip_ratio_left, slip_ratio_right);
float k = app_pid_compute(pid, SLIP_RATIO_IDEAL, slip_ratio_max);
float k;

// correcting based on most deviance from ideal torque

if(error_slip_left < error_slip_right)
{

k = app_pid_compute(pid, SLIP_RATIO_IDEAL, slip_ratio_right);
outputs->torque_right_Nm = inputs->torque_right_Nm - k;
outputs->torque_left_Nm = inputs->torque_left_Nm;
}
else
{
k = app_pid_compute(pid, SLIP_RATIO_IDEAL, slip_ratio_left);
outputs->torque_left_Nm = inputs->torque_left_Nm - k;
outputs->torque_right_Nm = inputs->torque_right_Nm;
}

// Send debug messages over CAN
app_canTx_VC_SlipRatioLeft_set(slip_ratio_left);
Expand All @@ -30,9 +48,6 @@ void app_tractionControl_computeTorque(TractionControl_Inputs *inputs, TractionC
app_canTx_VC_PIDSlipRatioDerivative_set(pid->derivative);
app_canTx_VC_PIDSlipRatioIntegral_set(pid->integral);

// NOTE: k strictly in range [-1 0] to prevent exceeding power limit
outputs->torque_left_final_Nm = (1.0f + k) * inputs->torque_left_Nm;
outputs->torque_right_final_Nm = (1.0f + k) * inputs->torque_right_Nm;
}

float app_tractionControl_computeSlip(float motor_speed_rpm, float front_wheel_speed_rpm)
Expand Down
11 changes: 9 additions & 2 deletions firmware/quadruna/VC/src/app/vehicle_dynamics/app_vehicleDynamics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ typedef struct TractionControl_Inputs
{
float torque_left_Nm;
float torque_right_Nm;
float torque_limit;
float motor_speed_left_rpm;
float motor_speed_right_rpm;
float wheel_speed_front_left_kph;
Expand All @@ -24,8 +25,8 @@ typedef struct TractionControl_Inputs

typedef struct TractionControl_Outputs
{
float torque_left_final_Nm;
float torque_right_final_Nm;
float torque_left_Nm;
float torque_right_Nm;
} TractionControl_Outputs;

typedef struct ActiveDifferential_Inputs
Expand All @@ -51,3 +52,9 @@ typedef struct PowerLimiting_Inputs
const float power_limit_kW;
float accelerator_pedal_percent;
} PowerLimiting_Inputs;

typedef struct Torque_to_Inverters
{
float torque_left_final_Nm;
float torque_right_final_Nm;
}Torque_to_Inverters;

0 comments on commit 526083c

Please sign in to comment.