Skip to content

Commit

Permalink
Merge pull request #69 from hytech-racing/hotfix_pedals
Browse files Browse the repository at this point in the history 
Tested on car and with unit testing. Merging into master.
  • Loading branch information
@walkermburns
walkermburns committed Apr 11, 2024
2 parents 84a65b0 + bb794f2 commit d5f106a
Show file tree
Hide file tree
Showing 6 changed files with 502 additions and 210 deletions.
8 changes: 4 additions & 4 deletions include/MCU_rev15_defs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,12 +51,12 @@ const int BRAKE2_PEDAL_MAX = 2198;
const int BRAKE1_PEDAL_MIN = 867;
const int BRAKE2_PEDAL_MIN = 867;

const float DEFAULT_PEDAL_DEADZONE = 0.05f;
const float DEFAULT_PEDAL_DEADZONE = 0.03f;
const float DEFAULT_PEDAL_IMPLAUSIBILITY_MARGIN = 0.10f;

const float APPS_ACTIVATION_PERCENTAGE = 0.1;
const float BRAKE_ACTIVATION_PERCENTAGE = 0.05;
const float BRAKE_MECH_THRESH = 0.40;
const float APPS_ACTIVATION_PERCENTAGE = 0.1f;
const float BRAKE_ACTIVATION_PERCENTAGE = 0.05f;
const float BRAKE_MECH_THRESH = 0.40f;

// Load Cell Defs to convert raw to lbs
// lbs = (scale)*raw + offset
Expand Down
8 changes: 5 additions & 3 deletions lib/state_machine/include/MCUStateMachine.tpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,14 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
// Serial.print(" ");
// Serial.print(data.brakeAndAccelPressedImplausibility);
// Serial.print(" ");
// Serial.print(data.implausibilityExceededMaxDuration);
// Serial.println();

// Serial.println("accel, brake:");
// Serial.print(data.accelPercent);
// Serial.print(" ");
// Serial.print(data.brakePercent);
// Serial.print(" ");
// Serial.print(" \n");
// Serial.print(data.implausibilityExceededMaxDuration);
// Serial.println();


// if TS is above HV threshold, move to Tractive System Active
Expand Down
19 changes: 15 additions & 4 deletions lib/systems/include/PedalsSystem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ struct PedalsSystemData_s
bool accelImplausible : 1;
bool brakeImplausible : 1;
bool brakePressed : 1;
bool accelPressed : 1;
bool mechBrakeActive : 1;
bool brakeAndAccelPressedImplausibility : 1;
bool implausibilityExceededMaxDuration : 1;
Expand Down Expand Up @@ -42,10 +43,16 @@ class PedalsSystem
/// @param brakeParams brake pedal params. when used with only one pedal sensor, the pedal parameter evaluation for brakes only looks at the min and max for min_pedal_1 / max_pedal_1
PedalsSystem(const PedalsParams &accelParams,
const PedalsParams &brakeParams)
{
setParams(accelParams, brakeParams);
implausibilityStartTime_ = 0;
}

void setParams(const PedalsParams &accelParams,
const PedalsParams &brakeParams)
{
accelParams_ = accelParams;
brakeParams_ = brakeParams;
implausibilityStartTime_ = 0;
}

const PedalsSystemData_s &getPedalsSystemData()
Expand Down Expand Up @@ -115,8 +122,6 @@ class PedalsSystem
PedalsParams brakeParams_{};
unsigned long implausibilityStartTime_;

bool both_pedals_implausible;

float remove_deadzone_(float conversion_input, float deadzone);
bool max_duration_of_implausibility_exceeded_(unsigned long curr_time);

Expand Down Expand Up @@ -179,7 +184,13 @@ class PedalsSystem
int max,
float implaus_margin_scale);

bool pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, float percent_threshold);
/// @brief check whether or not pedal is active according to input parameters. returns true if either pedal is over threshold. removes the deadzone before checking.
/// @param pedal1ConvertedData the value 0 to 1 of the first pedal without deadzone removed
/// @param pedal2ConvertedData ... second pedal 0 to 1 val
/// @param params the pedal parameters for this specific pedal
/// @param check_mech_activation if this is true, function will check percentages against the mechanical activation percentage
/// @return true or false accordingly
bool pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, const PedalsParams &params, bool check_mech_activation);
};

#endif /* PEDALSSYSTEM */
106 changes: 52 additions & 54 deletions lib/systems/src/PedalsSystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,21 @@ void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1,
data_ = evaluate_pedals(accel1, accel2, brake, tick.millis);
}


// TODO make it to where only when the pedal is released fully that the implausibility clears


PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel1,
const AnalogConversion_s &accel2,
const AnalogConversion_s &brake,
unsigned long curr_time)
{
PedalsSystemData_s out;

out.accelPercent = ((accel1.conversion + accel2.conversion) / 2.0);
out.accelPercent = remove_deadzone_(out.accelPercent, accelParams_.deadzone_margin);
out.accelPercent = std::max(out.accelPercent, 0.0f);
out.accelPressed = pedal_is_active_(accel1.conversion, accel2.conversion, accelParams_, false);
out.accelImplausible = evaluate_pedal_implausibilities_(accel1, accel2, accelParams_, 0.1);
out.brakeImplausible = evaluate_pedal_implausibilities_(brake, brakeParams_);
out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(accel1, accel2, brake);
Expand All @@ -29,20 +38,20 @@ PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel
{
implausibilityStartTime_ = curr_time;
}
else if (!implausibility)
else if ((!implausibility) && ((out.accelPercent <= 0.05)))
{
implausibilityStartTime_ = 0;
}

out.accelPercent = (accel1.conversion + accel2.conversion) / 2.0;
out.accelPercent = remove_deadzone_(out.accelPercent, accelParams_.deadzone_margin);

out.brakePercent = brake.conversion;
out.brakePercent = remove_deadzone_(out.brakePercent, brakeParams_.deadzone_margin);
out.brakePressed = brake.conversion >= brakeParams_.activation_percentage;

out.mechBrakeActive = out.brakePercent >= brakeParams_.mechanical_activation_percentage;
out.regenPercent = std::max(std::min(out.brakePercent / brakeParams_.mechanical_activation_percentage, 1.0f), 0.0f);
out.mechBrakeActive = out.brakePercent > brakeParams_.mechanical_activation_percentage;


out.implausibilityExceededMaxDuration = max_duration_of_implausibility_exceeded_(curr_time);
return out;
}
Expand All @@ -55,7 +64,14 @@ PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel
{

PedalsSystemData_s out;
out.accelPressed = pedal_is_active_(accel1.conversion, accel2.conversion, accelParams_, false);
out.accelImplausible = evaluate_pedal_implausibilities_(accel1, accel2, accelParams_, 0.1);

auto percent = (accel1.conversion + accel2.conversion) / 2.0;
out.accelPercent = remove_deadzone_(percent, accelParams_.deadzone_margin);
out.accelPercent = std::max(out.accelPercent, 0.0f);


out.brakeImplausible = evaluate_pedal_implausibilities_(brake1, brake2, brakeParams_, 0.25);
out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(accel1, accel2, brake1, brake2);
bool implausibility = (out.brakeAndAccelPressedImplausibility || out.brakeImplausible || out.accelImplausible);
Expand All @@ -64,19 +80,20 @@ PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel
{
implausibilityStartTime_ = curr_time;
}
else if (!implausibility)
else if ((!implausibility) && (!(out.accelPercent > 0.05)))
{
implausibilityStartTime_ = 0;
}

out.accelPercent = (accel1.conversion + accel2.conversion) / 2.0;

out.brakePercent = (brake1.conversion + brake2.conversion) / 2.0;
out.accelPercent = remove_deadzone_(out.accelPercent, accelParams_.deadzone_margin);

out.brakePercent = remove_deadzone_(out.brakePercent, brakeParams_.deadzone_margin);

out.regenPercent = std::max(std::min(out.brakePercent / brakeParams_.mechanical_activation_percentage, 1.0f), 0.0f);
out.brakePressed = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_.activation_percentage);
out.mechBrakeActive = out.brakePercent > brakeParams_.mechanical_activation_percentage;

out.brakePressed = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_, false);
out.mechBrakeActive = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_, true);
out.implausibilityExceededMaxDuration = max_duration_of_implausibility_exceeded_(curr_time);

return out;
Expand All @@ -98,17 +115,7 @@ bool PedalsSystem::max_duration_of_implausibility_exceeded_(unsigned long curr_t
// only checks implaus based on first min / max
bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pedalData, const PedalsParams &params)
{
bool pedal1_min_max_implaus = evaluate_min_max_pedal_implausibilities_(pedalData, params.min_pedal_1, params.max_pedal_1, params.implausibility_margin);


if (pedal1_min_max_implaus)
{
return true;
}
else
{
return false;
}
return evaluate_min_max_pedal_implausibilities_(pedalData, params.min_pedal_1, params.max_pedal_1, params.implausibility_margin);
}

bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pedalData1,
Expand Down Expand Up @@ -192,22 +199,13 @@ bool PedalsSystem::evaluate_brake_and_accel_pressed_(const AnalogConversion_s &a
const AnalogConversion_s &brakePedalData)
{

float accel_pedal1_real = remove_deadzone_(accelPedalData1.conversion, accelParams_.deadzone_margin);
float accel_pedal2_real = remove_deadzone_(accelPedalData2.conversion, accelParams_.deadzone_margin);


bool accel_pressed = pedal_is_active_(accelPedalData1.conversion, accelPedalData2.conversion, accelParams_, false); // .1
float brake_pedal_real = remove_deadzone_(brakePedalData.conversion, brakeParams_.deadzone_margin);
bool accel_pressed = pedal_is_active_(accel_pedal1_real, accel_pedal2_real, accelParams_.activation_percentage); // .1
bool brake_pressed = brake_pedal_real >= brakeParams_.mechanical_activation_percentage;

if (accel_pressed && brake_pressed) {
both_pedals_implausible = true;
}

// make sure that implausibility does not clear until both pedals are completely depressed
if (!accel_pressed && !brake_pressed) {
both_pedals_implausible = false;
}

return (both_pedals_implausible);
bool mech_brake_pressed = brake_pedal_real >= brakeParams_.mechanical_activation_percentage;
bool both_pedals_implausible = (accel_pressed && mech_brake_pressed);
return both_pedals_implausible;
}

bool PedalsSystem::evaluate_brake_and_accel_pressed_(const AnalogConversion_s &accelPedalData1,
Expand All @@ -216,28 +214,28 @@ bool PedalsSystem::evaluate_brake_and_accel_pressed_(const AnalogConversion_s &a
const AnalogConversion_s &brakePedalData2)
{

float accel_pedal1_real = remove_deadzone_(accelPedalData1.conversion, accelParams_.deadzone_margin);
float accel_pedal2_real = remove_deadzone_(accelPedalData2.conversion, accelParams_.deadzone_margin);
float brake_pedal1_real = remove_deadzone_(brakePedalData1.conversion, brakeParams_.deadzone_margin);
float brake_pedal2_real = remove_deadzone_(brakePedalData2.conversion, brakeParams_.deadzone_margin);
bool accel_pressed = pedal_is_active_(accel_pedal1_real, accel_pedal2_real, accelParams_.activation_percentage); // .1
bool brake_pressed = pedal_is_active_(brake_pedal1_real, brake_pedal2_real, brakeParams_.mechanical_activation_percentage); // 0.05

if (accel_pressed && brake_pressed) {
both_pedals_implausible = true;
}

// make sure that implausibility does not clear until both pedals are completely depressed
if (!accel_pressed && !brake_pressed) {
both_pedals_implausible = false;
}

return (both_pedals_implausible);
bool accel_pressed = pedal_is_active_(accelPedalData1.conversion, accelPedalData2.conversion, accelParams_, false); // .1
bool mech_brake_pressed = pedal_is_active_(brakePedalData1.conversion, brakePedalData2.conversion, brakeParams_, true); // 0.40


bool both_pedals_implausible = (accel_pressed && mech_brake_pressed);
return both_pedals_implausible;
}

bool PedalsSystem::pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, float percent_threshold)
bool PedalsSystem::pedal_is_active_(float pedal1ConvertedData, float pedal2ConvertedData, const PedalsParams& params, bool check_mech_activation)
{
bool pedal_1_is_active = pedal1ConvertedData >= percent_threshold;
bool pedal_2_is_active = pedal2ConvertedData >= percent_threshold;
float val1_deadzone_removed = remove_deadzone_(pedal1ConvertedData, params.deadzone_margin);
float val2_deadzone_removed = remove_deadzone_(pedal2ConvertedData, params.deadzone_margin);
bool pedal_1_is_active;
bool pedal_2_is_active;
if(check_mech_activation)
{
pedal_1_is_active = val1_deadzone_removed >= params.mechanical_activation_percentage;
pedal_2_is_active = val2_deadzone_removed >= params.mechanical_activation_percentage;
} else {
pedal_1_is_active = val1_deadzone_removed >= params.activation_percentage;
pedal_2_is_active = val2_deadzone_removed >= params.activation_percentage;
}
return (pedal_1_is_active || pedal_2_is_active);
}
1 change: 0 additions & 1 deletion src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,6 @@ void loop()
// reset inverters
if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured())
{
hal_println("resetting errored drivetrain");
drivetrain.reset_drivetrain();
}
// tick state machine
Expand Down
Loading

0 comments on commit d5f106a

Please sign in to comment.