diff --git a/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.c b/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.c index e71d70dffe..7092ddf29d 100644 --- a/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.c +++ b/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.c @@ -11,20 +11,29 @@ static PowerLimitingInputs powerLimitingInputs; -void app_wheelVerticalForces_broadcast(const ImuData *imu_data) +void app_wheelVerticalForces_broadcast(const ImuData *imu_data, TractionControl_Inputs *inputs) { + float drag_lt_per_wheel = DRAG_PITCH_TERM_VERTICAL_FORCE(inputs->vehicle_velocity_kmh) / 2.0f; app_canTx_VC_FrontLeftWheelVerticalForce_set( - (uint32_t)(((REAR_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel))); + (uint32_t)(((FRONT_AXLE_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 2.0f) - + (DOWNFORCE_TERM_VERTICAL_FORCE(inputs->vehicle_velocity_kmh) * + STATIC_COP_FRONT(imu_data->long_accel) * STATIC_COP_LEFT(imu_data->lat_accel)) - + LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel) - drag_lt_per_wheel)); app_canTx_VC_FrontRightWheelVerticalForce_set( - (uint32_t)(((REAR_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel))); + (uint32_t)(((FRONT_AXLE_WEIGHT_DISTRIBUTION - LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 2.0f) + + (DOWNFORCE_TERM_VERTICAL_FORCE(inputs->vehicle_velocity_kmh) * + STATIC_COP_FRONT(imu_data->long_accel) * STATIC_COP_RIGHT(imu_data->lat_accel)) + + LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel) - drag_lt_per_wheel)); app_canTx_VC_RearLeftWheelVerticalForce_set( - (uint32_t)(((REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 4.0f) - - LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel))); + (uint32_t)(((REAR_AXLE_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 2.0f) - + (DOWNFORCE_TERM_VERTICAL_FORCE(inputs->vehicle_velocity_kmh) * + STATIC_COP_REAR(imu_data->long_accel) * STATIC_COP_LEFT(imu_data->lat_accel)) - + LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel) + drag_lt_per_wheel)); app_canTx_VC_RearRightWheelVerticalForce_set( - (uint32_t)(((REAR_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 4.0f) + - LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel))); + (uint32_t)(((REAR_AXLE_WEIGHT_DISTRIBUTION + LONG_ACCEL_TERM_VERTICAL_FORCE(imu_data->long_accel)) / 2.0f) + + (DOWNFORCE_TERM_VERTICAL_FORCE(inputs->vehicle_velocity_kmh) * + STATIC_COP_REAR(imu_data->long_accel) * STATIC_COP_RIGHT(imu_data->lat_accel)) + + LAT_ACCEL_TERM_VERTICAL_FORCE(imu_data->lat_accel) + drag_lt_per_wheel)); } float app_loadTransferConstant(const float long_accel) @@ -50,14 +59,18 @@ void app_torqueAllocation(TorqueAllocationInputs *inputs, TorqueAllocationOutput // percentage of max torque of motor not of the car // potential divisino by 0 if load_transfer_const = -1, not sure what values are expected here but should add safety + + float drag_comp = DRAG_COMPENSATION_TORQUE_NM(inputs->vehicle_velocity_kmh); + torqueToMotors->front_left_torque = (inputs->total_torque_request * inputs->load_transfer_const) / (2 * (inputs->load_transfer_const + 1)) - - (inputs->front_yaw_moment / (2 * F)); + (inputs->front_yaw_moment / (2 * F)) + drag_comp; torqueToMotors->front_right_torque = (torqueToMotors->front_left_torque + (inputs->front_yaw_moment / F)); torqueToMotors->rear_left_torque = - (inputs->total_torque_request / 2) - - (inputs->total_torque_request * inputs->load_transfer_const) / (2 * (inputs->load_transfer_const + 1)); + ((inputs->total_torque_request / 2) - + (inputs->total_torque_request * inputs->load_transfer_const) / (2 * (inputs->load_transfer_const + 1))) + + drag_comp; torqueToMotors->rear_right_torque = (torqueToMotors->rear_left_torque + (inputs->rear_yaw_moment / F)); powerLimitingInputs.total_requestedPower = app_totalPower(torqueToMotors); diff --git a/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.h b/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.h index 5cc8286eac..3c0676bd23 100644 --- a/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.h +++ b/firmware/quintuna/VC/src/app/vehicle_dynamics/app_torqueDistribution.h @@ -7,7 +7,7 @@ * Broadcasts the vertical force of each wheel * calculated based on IMU accelerations */ -void app_wheelVerticalForces_broadcast(const ImuData *imu); +void app_wheelVerticalForces_broadcast(const ImuData *imu, TractionControl_Inputs *inputs); /* * Calculates the load transfer constant needed to distribute torque diff --git a/firmware/quintuna/VC/src/app/vehicle_dynamics/app_vehicleDynamicsConstants.h b/firmware/quintuna/VC/src/app/vehicle_dynamics/app_vehicleDynamicsConstants.h index 75b7a19d17..15c1a16d42 100644 --- a/firmware/quintuna/VC/src/app/vehicle_dynamics/app_vehicleDynamicsConstants.h +++ b/firmware/quintuna/VC/src/app/vehicle_dynamics/app_vehicleDynamicsConstants.h @@ -31,6 +31,15 @@ #define MAX_REGEN_Nm -15.0f #define MAX_BATTERY_TEMP 45 #define POWER_LIMIT_REGEN_kW 17.0f // 17.64kW ~ 30A charge for molicel cells +#define FRONTAL_AREA 0.94f // m^2 from aero team +#define AIR_DENSITY 1.2205f // kg/m^3 +#define COEFFICIENT_OF_LIFT 1.7f // from Aero team +#define COEFFICIENT_OF_DRAG 1.1f // from Aero team +#define COP_REAR 0.68f // from Aero team +#define COP_RIGHT 0.5f // Assumed symmetric. Aero team has not characterized lateral CoP +#define COP_HEIGHT (42.0f * CM_TO_M) +#define WHEEL_RADIUS_m ((WHEEL_DIAMETER_IN / 2.0f) * 0.0254f) +#define KMH_TO_MS (1.0f / 3.6f) /** * Reference used: https://www.zotero.org/groups/5809911/vehicle_controls_2024/items/N4TQBR67/reader @@ -39,13 +48,34 @@ #define DIST_FRONT_AXLE_CG (0.837f) // a in meters #define DIST_REAR_AXLE_CG (WHEELBASE_m - DIST_FRONT_AXLE_CG) // b in meters #define WEIGHT_ACROSS_BODY (CAR_MASS_AT_CG_KG * GRAVITY / WHEELBASE_m) +#define DOWNFORCE_CONSTANT (AIR_DENSITY * FRONTAL_AREA * COEFFICIENT_OF_LIFT / 2.0f) +#define DRAG_CONSTANT (AIR_DENSITY * FRONTAL_AREA * COEFFICIENT_OF_DRAG / 2.0f) /************** Macros for finding vertical forces on wheels based on diagram on page 21 ****************/ -#define REAR_WEIGHT_DISTRIBUTION (WEIGHT_ACROSS_BODY * DIST_REAR_AXLE_CG) +#define FRONT_AXLE_WEIGHT_DISTRIBUTION (WEIGHT_ACROSS_BODY * DIST_REAR_AXLE_CG) +#define REAR_AXLE_WEIGHT_DISTRIBUTION (WEIGHT_ACROSS_BODY * DIST_FRONT_AXLE_CG) #define LONG_ACCEL_TERM_VERTICAL_FORCE(long_accel) \ ((CAR_MASS_AT_CG_KG * (long_accel) * CG_HEIGHT_FROM_GROUND_m / WHEELBASE_m)) #define LAT_ACCEL_TERM_VERTICAL_FORCE(lat_accel) \ ((CAR_MASS_AT_CG_KG * (lat_accel) * CG_HEIGHT_FROM_GROUND_m / (2.0f * (TRACK_WIDTH_m)))) +#define DOWNFORCE_TERM_VERTICAL_FORCE(vehicle_velocity_kmh) \ + ((DOWNFORCE_CONSTANT) * (vehicle_velocity_kmh * KMH_TO_MS) * (vehicle_velocity_kmh * KMH_TO_MS)) + +// Longitudinal CoP shift with pitch not modeled — aero team to provide data next year + +#define STATIC_COP_FRONT(long_accel) (1 - (COP_REAR)) +#define STATIC_COP_REAR(long_accel) (COP_REAR) +#define STATIC_COP_LEFT(lat_accel) (COP_RIGHT) +#define STATIC_COP_RIGHT(lat_accel) (1 - (COP_RIGHT)) + +#define DRAG_FORCE_NEWTONS(vehicle_velocity_kmh) \ + ((DRAG_CONSTANT) * (vehicle_velocity_kmh * KMH_TO_MS) * (vehicle_velocity_kmh * KMH_TO_MS)) +#define DRAG_PITCH_TERM_VERTICAL_FORCE(vehicle_velocity_kmh) \ + ((DRAG_FORCE_NEWTONS(vehicle_velocity_kmh) * COP_HEIGHT / WHEELBASE_m)) + +// Torque required to counteract resistive drag force per motor (4WD) +#define DRAG_COMPENSATION_TORQUE_NM(vehicle_velocity_kmh) \ + ((DRAG_FORCE_NEWTONS(vehicle_velocity_kmh) * WHEEL_RADIUS_m) / 4.0f) /************** Macros for finding Kmz based on diagram on page 57 ****************/ #define ACCELERATION_TERM_KMZ(long_accel) (DIST_FRONT_AXLE_CG + (long_accel) * CG_HEIGHT_FROM_GROUND_m / GRAVITY)