Skip to content

Commit

Permalink
Good working draft of SendTritium MVP & MVP+Cruise
Browse files Browse the repository at this point in the history
  • Loading branch information
@OscarPortillo37
OscarPortillo37 committed Dec 7, 2024
1 parent a46b193 commit 6b77e98
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 114 deletions.
9 changes: 3 additions & 6 deletions Apps/Src/SendTritium_MVP.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,11 +60,6 @@ static float velocityObserved = 0;
// Counter for sending setpoints to motor
static uint8_t motorMsgCounter = 0;

// Debouncing counters
// static uint8_t onePedalCounter = 0;
// static uint8_t cruiseEnableCounter = 0;
// static uint8_t cruiseSetCounter = 0;

// FSM
static TritiumState_t prevState; // Previous state
static TritiumState_t state; // Current state
Expand Down Expand Up @@ -336,7 +331,7 @@ static void ParkHandler()
*/
static void ParkDecider()
{
// Switch for FORWARD_DRIVE/REVERSE_DRIVE if in FORWARD_GEAR/REVERSE_GEAR & speed is less than MAX_GEARSWITCH_VELOCITY
// Switch to FORWARD_DRIVE/REVERSE_DRIVE if in FORWARD_GEAR/REVERSE_GEAR & speed is less than MAX_GEARSWITCH_VELOCITY
if (gear == FORWARD_GEAR && (velocityObserved < 0 ? (velocityObserved >= -MAX_GEARSWITCH_VELOCITY) : (velocityObserved <= MAX_GEARSWITCH_VELOCITY)))
{
state = FSM[FORWARD_DRIVE];
Expand Down Expand Up @@ -399,6 +394,7 @@ void Task_SendTritium(void *p_arg)
// Initialize current state to PARK_STATE
state = FSM[PARK_STATE];
prevState = FSM[PARK_STATE];
UpdateDisplay_SetGear(PARK_GEAR);

// Initialize Regen & Cruise disabled
// NOTE: Regen & Display stay disabled on Daybreak MVP
Expand Down Expand Up @@ -430,6 +426,7 @@ void Task_SendTritium(void *p_arg)
prevState = state;
state.stateDecider(); // decide what the next state is

// This ONLY works for MVP (since Cruise has velocity control)
// Disable velocity controlled mode by always overwriting velocity to the maximum
// in the appropriate direction.
velocitySetpoint = (velocitySetpoint > 0) ? MAX_VELOCITY : -MAX_VELOCITY;
Expand Down
159 changes: 51 additions & 108 deletions Apps/Src/SendTritium_MVP_Cruise.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@ static float velocityObserved = 0;
static uint8_t motorMsgCounter = 0;

// Debouncing counters
// static uint8_t cruiseEnableCounter = 0;
// static uint8_t cruiseSetCounter = 0;
static uint8_t cruiseEnableCounter = 0;
static uint8_t cruiseSetCounter = 0;

// Button states
static bool cruiseEnableButton = false;
Expand Down Expand Up @@ -231,11 +231,12 @@ static void readInputs()
accelPedalPercent = Pedals_Read(ACCELERATOR);

// Update buttons
// if(Minions_Read(CRUZ_EN) && cruiseEnableCounter < DEBOUNCE_PERIOD){cruiseEnableCounter++;}
// else if(cruiseEnableCounter > 0){cruiseEnableCounter--;}
// TODO
if(Minions_Read(CRUZ_EN) && cruiseEnableCounter < DEBOUNCE_PERIOD){cruiseEnableCounter++;}
else if(cruiseEnableCounter > 0){cruiseEnableCounter--;}

// if(Minions_Read(CRUZ_ST) && cruiseSetCounter < DEBOUNCE_PERIOD){cruiseSetCounter++;}
// else if(cruiseSetCounter > 0){cruiseSetCounter--;}
if(Minions_Read(CRUZ_ST) && cruiseSetCounter < DEBOUNCE_PERIOD){cruiseSetCounter++;}
else if(cruiseSetCounter > 0){cruiseSetCounter--;}

// Update gears
bool forwardSwitch = Minions_Read(FOR_SW);
Expand Down Expand Up @@ -277,18 +278,22 @@ static void readInputs()
UpdateDisplay_SetGear(DISP_REVERSE);

// Debouncing
// TODO
cruiseEnableButton = false;
cruiseSet = false;
// if(cruiseEnableCounter == DEBOUNCE_PERIOD){cruiseEnableButton = true;}
// else if(cruiseEnableCounter == 0){cruiseEnableButton = false;}
if(cruiseEnableCounter == DEBOUNCE_PERIOD){cruiseEnableButton = true;}
else if(cruiseEnableCounter == 0){cruiseEnableButton = false;}

// if(cruiseSetCounter == DEBOUNCE_PERIOD){cruiseSet = true;}
// else if(cruiseSetCounter == 0){cruiseSet = false;}
if(cruiseSetCounter == DEBOUNCE_PERIOD){cruiseSet = true;}
else if(cruiseSetCounter == 0){cruiseSet = false;}

// Toggle
// if(cruiseEnableButton != cruiseEnablePrevious && cruiseEnablePrevious){cruiseEnable = !cruiseEnable;}
// cruiseEnablePrevious = cruiseEnableButton;
cruiseEnablePrevious = false;
if(cruiseEnableButton != cruiseEnablePrevious && cruiseEnablePrevious) // Falling edge CRUZ_EN detection
{
cruiseEnable = !cruiseEnable;
}
cruiseEnablePrevious = cruiseEnableButton;
// cruiseEnablePrevious = false;

// Get observed velocity
velocityObserved = Motor_RPM_Get();
Expand Down Expand Up @@ -361,6 +366,8 @@ static void ForwardDriveHandler()
Minions_Write(BRAKELIGHT, false);
UpdateDisplay_SetBrake(false);
}

cruiseEnable = false;
}

/**
Expand Down Expand Up @@ -463,48 +470,6 @@ static void ReverseDriveDecider()
// Otherwise, stays in REVERSE_DRIVE
}











































/**
* @brief Record Velocity State. While pressing the cruise set button,
* the car will record the observed velocity into velocitySetpoint.
Expand All @@ -523,27 +488,21 @@ static void RecordVelocityHandler()

/**
* @brief Record Velocity State Decider. Determines transitions out of record velocity
* state (brake, neutral drive, one pedal, forward drive, powered cruise).
* state (park, forward drive, powered cruise).
*/
static void RecordVelocityDecider()
{
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
{
state = FSM[BRAKE_STATE];
}
else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR)
{
state = FSM[NEUTRAL_DRIVE];
}
else if (onePedalEnable)
// If you're no longer in FORWARD_GEAR or you're braking, exit cruise
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD || gear == PARK_STATE || gear == REVERSE_GEAR)
{
cruiseEnable = false;
state = FSM[ONEPEDAL];
state = FSM[PARK_STATE];
}
// If cruise has been disabled, return to forward drive
else if (!cruiseEnable)
{
state = FSM[FORWARD_DRIVE];
}
// In cruise. Once the cruise set button has been unpressed, enter POWERED_CRUISE.
else if (cruiseEnable && !cruiseSet)
{
state = FSM[POWERED_CRUISE];
Expand All @@ -562,28 +521,22 @@ static void PoweredCruiseHandler()

/**
* @brief Powered Cruise State Decider. Determines transitions out of powered
* cruise state (brake, neutral drive, one pedal, forward drive, record velocity,
* accelerate cruise, coasting cruise).
* cruise state (park, forward drive, record velocity, accelerate cruise,
* coasting cruise).
*/
static void PoweredCruiseDecider()
{
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
// If you're no longer in FORWARD_GEAR or you're braking, exit cruise
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD || gear == PARK_STATE || gear == REVERSE_GEAR)
{
state = FSM[BRAKE_STATE];
}
else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR)
{
state = FSM[NEUTRAL_DRIVE];
}
else if (onePedalEnable)
{
cruiseEnable = false;
state = FSM[ONEPEDAL];
state = FSM[PARK_STATE];
}
// If cruise has been disabled, return to forward drive
else if (!cruiseEnable)
{
state = FSM[FORWARD_DRIVE];
}
// If CRUISE_SET to a different velocity, return to RECORD_VELOCITY
else if (cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY)
{
state = FSM[RECORD_VELOCITY];
Expand Down Expand Up @@ -616,23 +569,17 @@ static void CoastingCruiseHandler()
*/
static void CoastingCruiseDecider()
{
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
{
state = FSM[BRAKE_STATE];
}
else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR)
// If you're no longer in FORWARD_GEAR or you're braking, exit cruise
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD || gear == PARK_STATE || gear == REVERSE_GEAR)
{
state = FSM[NEUTRAL_DRIVE];
}
else if (onePedalEnable)
{
cruiseEnable = false;
state = FSM[ONEPEDAL];
state = FSM[PARK_STATE];
}
// If cruise has been disabled, return to forward drive
else if (!cruiseEnable)
{
state = FSM[FORWARD_DRIVE];
}
// If CRUISE_SET to a different velocity, return to RECORD_VELOCITY
else if (cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY)
{
state = FSM[RECORD_VELOCITY];
Expand Down Expand Up @@ -665,23 +612,17 @@ static void AccelerateCruiseHandler()
*/
static void AccelerateCruiseDecider()
{
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
{
state = FSM[BRAKE_STATE];
}
else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR)
{
state = FSM[NEUTRAL_DRIVE];
}
else if (onePedalEnable)
// If you're no longer in FORWARD_GEAR or you're braking, exit cruise
if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD || gear == PARK_STATE || gear == REVERSE_GEAR)
{
cruiseEnable = false;
state = FSM[ONEPEDAL];
state = FSM[PARK_STATE];
}
// If cruise has been disabled, return to forward drive
else if (!cruiseEnable)
{
state = FSM[FORWARD_DRIVE];
}
// If CRUISE_SET to a different velocity, return to RECORD_VELOCITY
else if (cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY)
{
state = FSM[RECORD_VELOCITY];
Expand All @@ -701,9 +642,10 @@ void Task_SendTritium(void *p_arg)
{
OS_ERR err;

// Initialize current state to FORWARD_DRIVE
state = FSM[NEUTRAL_DRIVE];
prevState = FSM[NEUTRAL_DRIVE];
// Initialize current state to PARK_STATE
state = FSM[PARK_STATE];
prevState = FSM[PARK_STATE];
UpdateDisplay_SetGear(PARK_GEAR);

// Initialize Regen & Cruise disabled
// NOTE: Regen stays disabled on Daybreak MVP
Expand Down Expand Up @@ -735,9 +677,10 @@ void Task_SendTritium(void *p_arg)
prevState = state;
state.stateDecider(); // decide what the next state is

// Disable velocity controlled mode by always overwriting velocity to the maximum
// in the appropriate direction.
velocitySetpoint = (velocitySetpoint > 0) ? MAX_VELOCITY : -MAX_VELOCITY;
// This ONLY works for MVP (since Cruise has velocity control)
// // Disable velocity controlled mode by always overwriting velocity to the maximum
// // in the appropriate direction.
// velocitySetpoint = (velocitySetpoint > 0) ? MAX_VELOCITY : -MAX_VELOCITY;

// Drive
#ifdef SENDTRITIUM_PRINT_MES
Expand Down

0 comments on commit 6b77e98

Please sign in to comment.