Updated MVP FSM. Working on MVP+Cruise FSM

Apps/Inc/SendTritium.h

@@ -99,4 +99,4 @@ EXPOSE_SETTER(float, velocitySetpoint)
 #endif
 
 
-/* @} */
+/* @} */

Apps/Inc/SendTritium_MVP.h

@@ -52,15 +52,15 @@ extern Gear_t gear;
 extern TritiumState_t state;
 
 // Outputs
-extern float currentSetpoint;
-extern float velocitySetpoint;
+extern float velocityObserved;
 #endif
 
 // Getter functions for local variables in SendTritium.c
 EXPOSE_GETTER(uint8_t, brakePedalPercent)
 EXPOSE_GETTER(uint8_t, accelPedalPercent)
 EXPOSE_GETTER(Gear_t, gear)
 EXPOSE_GETTER(TritiumState_t, state)
+EXPOSE_GETTER(float, velocityObserved)
 EXPOSE_GETTER(float, currentSetpoint)
 EXPOSE_GETTER(float, velocitySetpoint)
 
@@ -70,6 +70,7 @@ EXPOSE_SETTER(uint8_t, brakePedalPercent)
 EXPOSE_SETTER(uint8_t, accelPedalPercent)
 EXPOSE_SETTER(Gear_t, gear)
 EXPOSE_SETTER(TritiumState_t, state)
+EXPOSE_SETTER(float, velocityObserved)
 EXPOSE_SETTER(float, currentSetpoint)
 EXPOSE_SETTER(float, velocitySetpoint)
 #endif

Apps/Inc/SendTritium_MVP_Cruise.h

@@ -21,7 +21,7 @@
 
 #define FOREACH_Gear(GEAR) \
         GEAR(FORWARD_GEAR),   \
-        GEAR(NEUTRAL_GEAR),  \
+        GEAR(PARK_GEAR),  \
         GEAR(REVERSE_GEAR),   \
 
 typedef enum GEAR_ENUM {
@@ -32,13 +32,11 @@ typedef enum GEAR_ENUM {
 // State Names
 typedef enum{
     FORWARD_DRIVE,
-    NEUTRAL_DRIVE,
+    PARK_STATE,
     REVERSE_DRIVE,
     RECORD_VELOCITY,
     POWERED_CRUISE,
     COASTING_CRUISE,
-    BRAKE_STATE,
-    ONEPEDAL,
     ACCELERATE_CRUISE
 } TritiumStateName_t;
 
@@ -53,8 +51,6 @@ typedef struct TritiumState{
 // Inputs
 extern bool cruiseEnable;
 extern bool cruiseSet;
-extern bool onePedalEnable;
-extern bool regenEnable;
 
 extern uint8_t brakePedalPercent;
 extern uint8_t accelPedalPercent;
@@ -69,8 +65,6 @@ extern float cruiseVelSetpoint;
 // Getter functions for local variables in SendTritium.c
 EXPOSE_GETTER(bool, cruiseEnable)
 EXPOSE_GETTER(bool, cruiseSet)
-EXPOSE_GETTER(bool, onePedalEnable)
-EXPOSE_GETTER(bool, regenEnable)
 EXPOSE_GETTER(uint8_t, brakePedalPercent)
 EXPOSE_GETTER(uint8_t, accelPedalPercent)
 EXPOSE_GETTER(Gear_t, gear)
@@ -84,8 +78,6 @@ EXPOSE_GETTER(float, velocitySetpoint)
 #ifdef SENDTRITIUM_EXPOSE_VARS
 EXPOSE_SETTER(bool, cruiseEnable)
 EXPOSE_SETTER(bool, cruiseSet)
-EXPOSE_SETTER(bool, onePedalEnable)
-EXPOSE_SETTER(bool, regenEnable)
 EXPOSE_SETTER(uint8_t, brakePedalPercent)
 EXPOSE_SETTER(uint8_t, accelPedalPercent)
 EXPOSE_SETTER(Gear_t, gear)

Apps/Src/DebugDump.c

@@ -8,8 +8,8 @@
 #include "common.h"
 #include <errno.h> 
 #include "Tasks.h"
-#include "SendTritium.h"
-
+// #include "SendTritium.h"
+#include "SendTritium_MVP.h"
 
 static const char *MINIONPIN_STRING[] = {
     FOREACH_PIN(GENERATE_STRING)
@@ -54,10 +54,10 @@ void Task_DebugDump(void* p_arg) {
         } 
 
         // Send Tritium variables
-        printf("Cruise Enable: %s\n\r", get_cruiseEnable() ? "true" : "false");
-        printf("Cruise Set: %s\n\r", get_cruiseSet() ? "true" : "false");
-        printf("One Pedal Enable: %s\n\r", get_onePedalEnable() ? "true" : "false");
-        printf("Regen Enable: %s\n\r", get_regenEnable() ? "true" : "false");
+        // printf("Cruise Enable: %s\n\r", get_cruiseEnable() ? "true" : "false");
+        // printf("Cruise Set: %s\n\r", get_cruiseSet() ? "true" : "false");
+        // printf("One Pedal Enable: %s\n\r", get_onePedalEnable() ? "true" : "false");
+        // printf("Regen Enable: %s\n\r", get_regenEnable() ? "true" : "false");
         printf("Pedal Brake Percent: %d\n\r", get_brakePedalPercent());
         printf("Pedal Accel Percent: %d\n\r", get_accelPedalPercent());
         printf("Current Gear: %s\n\r", GEAR_STRING[get_gear()]);

Apps/Src/SendTritium.c

@@ -854,4 +854,4 @@ void Task_SendTritium(void *p_arg)
             assertOSError(err);
         }
     }
-}
+}

Apps/Src/SendTritium_MVP.c

@@ -36,9 +36,6 @@
 #define BRAKE_PEDAL_THRESHOLD 50 // percent
 #define ACCEL_PEDAL_THRESHOLD 15 // percent
 
-#define ONEPEDAL_BRAKE_THRESHOLD 25   // percent
-#define ONEPEDAL_NEUTRAL_THRESHOLD 35 // percent
-
 #define PEDAL_MIN 0        // percent
 #define PEDAL_MAX 100      // percent
 #define CURRENT_SP_MIN 0   // percent
@@ -57,6 +54,9 @@ float currentSetpoint = 0;
 float velocitySetpoint = 0;
 float busCurrentSetPoint = 69.0f; // why is this float and not int?
 
+// Current observed velocity
+static float velocityObserved = 0;
+
 // Counter for sending setpoints to motor
 static uint8_t motorMsgCounter = 0;
 
@@ -74,6 +74,7 @@ GETTER(uint8_t, brakePedalPercent)
 GETTER(uint8_t, accelPedalPercent)
 GETTER(Gear_t, gear)
 GETTER(TritiumState_t, state)
+GETTER(float, velocityObserved)
 GETTER(float, currentSetpoint)
 GETTER(float, velocitySetpoint)
 
@@ -83,15 +84,16 @@ SETTER(uint8_t, brakePedalPercent)
 SETTER(uint8_t, accelPedalPercent)
 SETTER(Gear_t, gear)
 SETTER(TritiumState_t, state)
+SETTER(float, velocityObserved)
 SETTER(float, currentSetpoint)
 SETTER(float, velocitySetpoint)
 #endif
 
 // Handler & Decider Declarations
 static void ForwardDriveHandler(void);
 static void ForwardDriveDecider(void);
-static void ParkDriveHandler(void);
-static void ParkDriveDecider(void);
+static void ParkHandler(void);
+static void ParkDecider(void);
 static void ReverseDriveHandler(void);
 static void ReverseDriveDecider(void);
 
@@ -153,6 +155,7 @@ static void dumpInfo()
     printf("gear: %d\n\r", (uint8_t)gear);
     print_float("currentSetpoint: ", currentSetpoint);
     print_float("velocitySetpoint: ", velocitySetpoint);
+    print_float("velocityObserved: ", velocityObserved);
     printf("-------------------\n\r");
 }
 #endif
@@ -226,6 +229,9 @@ static void readInputs()
         UpdateDisplay_SetGear(DISP_FORWARD);
     else if (gear == REVERSE_GEAR)
         UpdateDisplay_SetGear(DISP_REVERSE);
+
+    // Get observed velocity
+    velocityObserved = Motor_RPM_Get();
 }
 #endif
 
@@ -273,63 +279,73 @@ static uint8_t map(uint8_t input, uint8_t in_min, uint8_t in_max, uint8_t out_mi
  */
 static void ForwardDriveHandler()
 {
-    velocitySetpoint = MAX_VELOCITY;
-    // printf("accelPedalPercent: %d\n\r", accelPedalPercent);
-    currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX));
-    // print_float("float(map(accelPedalPercent)): %d\n\r", currentSetpoint);
+    // If braking, set current to 0. Otherwise, set current based on accel
+    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) {
+        velocitySetpoint = MAX_VELOCITY;
+        currentSetpoint = 0;
+    } else {
+        velocitySetpoint = MAX_VELOCITY;
+        currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX));
+    }
+
+    // Turn brakelight on/off
+    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) {
+        Minions_Write(BRAKELIGHT, true);
+        UpdateDisplay_SetBrake(true);
+    } else {
+        Minions_Write(BRAKELIGHT, false);
+        UpdateDisplay_SetBrake(false);
+    }
 }
 
 /**
  * @brief Forward Drive State Decider. Determines transitions out of
- * forward drive state (brake, record velocity, one pedal, neutral drive).
+ * forward drive state (park).
  */
 static void ForwardDriveDecider()
 {
-    if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
+    // Go to PARK_STATE if you're in another gear
+    if (gear == PARK_GEAR || gear == REVERSE_GEAR)
     {
         state = FSM[PARK_STATE];
-        Minions_Write(BRAKELIGHT, true);
-    }
-    else if (gear == REVERSE_GEAR)
-    {
-        state = FSM[PARK_STATE]; // transitions to PARK_STATE to then transition to REVERSE_DRIVE
-        Minions_Write(BRAKELIGHT, false);
     }
     // Otherwise, stays in FORWARD_DRIVE
 }
 
 /**
- * @brief Neutral Drive State Handler. No current is sent to the motor.
+ * @brief Park State Handler. No current is sent to the motor.
  */
 static void ParkHandler()
 {
     velocitySetpoint = MAX_VELOCITY;
     currentSetpoint = 0.0f;
-    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
-        Minions_Write(BRAKELIGHT, true);    
+
+    // Turn brakelight on/off
+    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) {
+        Minions_Write(BRAKELIGHT, true);
+        UpdateDisplay_SetBrake(true);
+    } else {
+        Minions_Write(BRAKELIGHT, false);
+        UpdateDisplay_SetBrake(false);
+    }
 }
 
 /**
- * @brief Neutral Drive State Decider. Determines transitions out of
- * neutral drive state (brake, forward drive, reverse drive).
+ * @brief Park State Decider. Determines transitions out of
+ * neutral drive state (forward drive, reverse drive).
  */
 static void ParkDecider()
 {
-    if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
-    {
-        state = FSM[PARK_STATE];
-        Minions_Write(BRAKELIGHT, true);
-    }
-    else if (gear == FORWARD_GEAR)
+    // Switch for 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];
-        Minions_Write(BRAKELIGHT, false);
     }
-    else if (gear == REVERSE_GEAR)
+    else if (gear == REVERSE_GEAR && (velocityObserved < 0 ? (velocityObserved >= -MAX_GEARSWITCH_VELOCITY) : (velocityObserved <=MAX_GEARSWITCH_VELOCITY)))
     {
         state = FSM[REVERSE_DRIVE];
-        Minions_Write(BRAKELIGHT, false);
     }
+    // Otherwise, remain in PARK_STATE
 }
 
 /**
@@ -338,27 +354,35 @@ static void ParkDecider()
  */
 static void ReverseDriveHandler()
 {
-    velocitySetpoint = -MAX_VELOCITY;
-    currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX));
-    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
-        Minions_Write(BRAKELIGHT, true);    
+    // If braking, set current to 0. Otherwise, set current based on accel
+    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) {
+        velocitySetpoint = MAX_VELOCITY;
+        currentSetpoint = 0;
+    } else {
+        velocitySetpoint = -MAX_VELOCITY;
+        currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX));
+    }
+
+    // Turn brakelight on/off
+    if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) {
+        Minions_Write(BRAKELIGHT, true);
+        UpdateDisplay_SetBrake(true);
+    } else {
+        Minions_Write(BRAKELIGHT, false);
+        UpdateDisplay_SetBrake(false);
+    }  
 }
 
 /**
  * @brief Reverse Drive State Decider. Determines transitions out of
- * reverse drive state (brake, neutral drive).
+ * reverse drive state (park).
  */
 static void ReverseDriveDecider()
 {
-    if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD)
+    // Go to PARK_STATE if you're in another gear or if you're braking
+    if (gear == PARK_GEAR || gear == FORWARD_GEAR)
     {
         state = FSM[PARK_STATE];
-        Minions_Write(BRAKELIGHT, true);
-    }
-    else if (gear == FORWARD_GEAR)
-    {
-        state = FSM[PARK_STATE]; // transitions to PARK_STATE to then transition to FORWARD_DRIVE
-        Minions_Write(BRAKELIGHT, false);
     }
     // Otherwise, stays in REVERSE_DRIVE
 }
@@ -376,6 +400,11 @@ void Task_SendTritium(void *p_arg)
     state = FSM[PARK_STATE];
     prevState = FSM[PARK_STATE];
 
+    // Initialize Regen & Cruise disabled
+    // NOTE: Regen & Display stay disabled on Daybreak MVP
+    UpdateDisplay_SetRegenState(DISP_DISABLED);
+    UpdateDisplay_SetCruiseState(DISP_DISABLED);
+
 #ifndef SENDTRITIUM_EXPOSE_VARS
     CANDATA_t driveCmd = {
         .ID = MOTOR_DRIVE,