diff --git a/ECU/Application/Inc/StateUtils.h b/ECU/Application/Inc/StateUtils.h index 955b0a520..b3d56a790 100644 --- a/ECU/Application/Inc/StateUtils.h +++ b/ECU/Application/Inc/StateUtils.h @@ -16,8 +16,8 @@ uint32_t MillisecondsSinceBoot(void); #define REGEN_MIN_SPEED_MPH 3.106856f // 5 KPH -#define MAX_CURRENT_AMPS 375.0f // Determined by Ryan -#define MAX_REVERSE_CURRENT_AMPS 30.0f // TODO: Change as appropriate +#define MAX_FORWARD_CURRENT_AMPS 325.0f +#define MAX_REVERSE_CURRENT_AMPS 300.0f #define ECU_STATUS_MSG_PERIOD_MILLIS (1000) #define TRACTIVE_SYSTEM_MAX_PERMITTED_DISCHARGE_TIME_MILLIS (5000) diff --git a/ECU/Application/Src/CANdler.c b/ECU/Application/Src/CANdler.c index cd1bd97d7..85778c2cf 100644 --- a/ECU/Application/Src/CANdler.c +++ b/ECU/Application/Src/CANdler.c @@ -18,7 +18,7 @@ #define NUM_MOTOR_POLE_PAIRS 10 #define DRIVEN_SPROCKET_TEETH 51.0f #define DRIVING_SPROCKET_TEETH 19.0f -#define GEAR_RATIO (51.0f / 19.0f) +#define GEAR_RATIO (DRIVEN_SPROCKET_TEETH / DRIVING_SPROCKET_TEETH) void ReportBadMessageLength(GRCAN_BUS_ID bus_id, GRCAN_MSG_ID msg_id, GRCAN_NODE_ID sender_id) { diff --git a/ECU/Application/Src/StateTicks.c b/ECU/Application/Src/StateTicks.c index 2a78ae364..3b503393a 100644 --- a/ECU/Application/Src/StateTicks.c +++ b/ECU/Application/Src/StateTicks.c @@ -58,8 +58,8 @@ ECU_StateData stateLump = { .apps_1_max = 1897, .apps_2_max = 1926, // Regen - .regen_strength = 2, - .enable_regen = false, + .regen_strength = 2, // Not used + .enable_regen = true, .SDC_startup_condition = true}; static uint32_t millis_since_boot; @@ -258,38 +258,79 @@ void ECU_Drive_Active(ECU_StateData *stateData) float torque_request; bool apps_plausible = (millis_since_boot - last_apps_plausible_frame_millis) <= MAX_APPS_IMPLAUSIBLE_TIME_MS; + float pedal = CalcAccPedalTravel(stateData); + + uint16_t max_rev_current = 0; + switch (stateData->powerlevel) { + case 0: + max_rev_current = 20; + break; + case 1: + max_rev_current = 40; + break; + case 2: + max_rev_current = 60; + break; + case 3: + max_rev_current = 80; + break; + case 4: + max_rev_current = 100; + break; + case 5: + max_rev_current = 120; + break; + default: + LOGOMATIC("Invalid power level: %d. Defaulting to no regen.\n", stateData->powerlevel); + max_rev_current = 0; + break; + } + + uint16_t max_fwd_current = 0; + switch (stateData->powerlevel) { + case 0: + max_fwd_current = 300; + break; + case 1: + max_fwd_current = 300; + break; + case 2: + max_fwd_current = 300; + break; + case 3: + max_fwd_current = 300; + break; + case 4: + max_fwd_current = 300; + break; + case 5: + max_fwd_current = 300; + break; + default: + LOGOMATIC("Invalid power level: %d. Defaulting to no forward torque.\n", stateData->powerlevel); + max_fwd_current = 0; + break; + } + + if (pedal < 0.05f) { + torque_request = -max_rev_current; + } else if (pedal < 0.1f) { + torque_request = -max_rev_current * (1.0f - (pedal - 0.05f) / 0.05f); + } else if (pedal < 0.2f) { + torque_request = 0.0f; + } else { + torque_request = max_fwd_current * ((pedal - 0.2f) / 0.8f); + } + + if (!stateData->enable_regen || stateData->vehicle_speed_mph <= REGEN_MIN_SPEED_MPH) { + torque_request = fmaxf(torque_request, 0); + } + + torque_request = fminf(torque_request, MAX_FORWARD_CURRENT_AMPS); + torque_request = fmaxf(torque_request, -MAX_REVERSE_CURRENT_AMPS); + if (stateData->apps_bse_violation || !apps_plausible) { torque_request = 0; - } else if (stateData->enable_regen && PressingBrake(stateData) && stateData->vehicle_speed_mph > REGEN_MIN_SPEED_MPH) { - torque_request = -MIN_WITH_TYPES(CalcBrakePressure(stateData) / 5000.0f * stateData->regen_strength, 1.0f) * MAX_REVERSE_CURRENT_AMPS; - } else { - uint16_t max_current = 0; - // Chosen max current for different power level / torque maps - switch (stateData->powerlevel) { - case 0: - max_current = 100; - break; - case 1: - max_current = 200; - break; - case 2: - max_current = 250; - break; - case 3: - max_current = 300; - break; - case 4: - max_current = 325; - break; - case 5: - max_current = 350; - break; - default: - LOGOMATIC("Invalid power level: %d. Defaulting to no current.\n", stateData->powerlevel); - max_current = 0; - break; - } - torque_request = fminf(CalcAccPedalTravel(stateData) * max_current, MAX_CURRENT_AMPS); } static uint32_t last_can_inverter_request_millis = 0; @@ -298,7 +339,14 @@ void ECU_Drive_Active(ECU_StateData *stateData) ECU_CAN_Send(GRCAN_BUS_PRIMARY, GRCAN_GR_Inv, GRCAN_INV_CMD, &message, sizeof(message)); ECU_CAN_Send_DTI(DTI_CONTROL_12_CAN_ID, &message.drive_enable, 1); message.set_ac_current = torque_request * 10; - ECU_CAN_Send_DTI(DTI_CONTROL_1_CAN_ID, &message.set_ac_current, 2); + LOGOMATIC("%f\t\t%f\n", stateData->vehicle_speed_mph, torque_request); + + if (torque_request < 0) { + message.set_ac_current = torque_request * -10; + ECU_CAN_Send_DTI(DTI_CONTROL_2_CAN_ID, &message.set_ac_current, 2); + } else { + ECU_CAN_Send_DTI(DTI_CONTROL_1_CAN_ID, &message.set_ac_current, 2); + } last_can_inverter_request_millis = millis_since_boot; } }