#168 State Machine Cleanup (#189)

* #168 State Machine Cleanup * #168 Prettier * #168 Delegate Functional Logic * #168 Migrate to State Request * #168 Prettier
Northeastern-Electric-Racing · Jul 31, 2024 · 4362198 · 4362198
1 parent e4da055
commit 4362198
Show file tree

Hide file tree

Showing 6 changed files with 270 additions and 325 deletions.
diff --git a/Core/Inc/nero.h b/Core/Inc/nero.h
@@ -5,50 +5,11 @@
 #include "cmsis_os.h"
 #include "stm32f4xx_hal.h"
 
-typedef enum {
-	OFF,
-	PIT,
-	PERFORMANCE,
-	EFFICIENCY,
-	DEBUG,
-	CONFIGURATION,
-	FLAPPY_BIRD,
-	EXIT,
-	MAX_NERO_STATES
-} nero_menu_t;
-
-typedef struct {
-	nero_menu_t nero_index;
-	bool home_mode;
-} nero_state_t;
-
-/*
- * Attempts to increment the nero index and handles any exceptions
-*/
-void increment_nero_index();
-
-/*
- * Attempts to decrement the nero index and handles any exceptions
-*/
-void decrement_nero_index();
-
-/*
- * Tells the statemachine to track up and down movements
-*/
-void set_home_mode();
-
-/*
- * Tells NERO to select the current index if in home mode
-*/
-void select_nero_index();
-
 /*
  * Tells NERO the current MPH
 */
 void set_mph(int8_t new_mph);
 
-void set_nero_home_mode();
-
 /*
 * Emits the state of NERO
 */

diff --git a/Core/Inc/queues.h b/Core/Inc/queues.h
@@ -28,8 +28,6 @@ typedef struct {
 
 extern osMessageQueueId_t pedal_data_queue;
 
-extern osMessageQueueId_t break_state_queue;
-
 bool get_brake_state();
 
 #endif // QUEUES_H
diff --git a/Core/Inc/state_machine.h b/Core/Inc/state_machine.h
@@ -10,6 +10,7 @@
  * being sub states of the ACTIVE functional state
  */
 typedef enum { BOOT, READY, ACTIVE, FAULTED, MAX_FUNC_STATES } func_state_t;
+
 typedef enum {
 	NOT_DRIVING,
 	REVERSE,
@@ -19,21 +20,33 @@ typedef enum {
 	MAX_DRIVE_STATES
 } drive_state_t;
 
-extern osThreadId_t sm_director_handle;
-extern const osThreadAttr_t sm_director_attributes;
+typedef enum {
+	OFF,
+	PIT,
+	PERFORMANCE,
+	EFFICIENCY,
+	DEBUG,
+	CONFIGURATION,
+	FLAPPY_BIRD,
+	EXIT,
+	MAX_NERO_STATES
+} nero_menu_t;
 
 typedef struct {
-	enum { FUNCTIONAL, DRIVE } id;
+	nero_menu_t nero_index;
+	bool home_mode;
+} nero_state_t;
 
-	union {
-		func_state_t functional;
-		drive_state_t drive;
-	} state;
-} state_req_t;
+typedef struct {
+	func_state_t functional;
+	drive_state_t drive;
+	nero_state_t nero;
+} state_t;
 
-void vStateMachineDirector(void *pv_params);
+extern osThreadId_t sm_director_handle;
+extern const osThreadAttr_t sm_director_attributes;
 
-int queue_state_transition(state_req_t request);
+void vStateMachineDirector(void *pv_params);
 
 /* Retrieves the current functional state */
 func_state_t get_func_state();
@@ -44,4 +57,36 @@ func_state_t get_func_state();
  */
 drive_state_t get_drive_state();
 
+/*
+ * Retrieves the current nero state
+ */
+nero_state_t get_nero_state();
+
+/**
+ * Increments the nero index in the order of nero_menu_t which will be used to select a drive mode
+ */
+int increment_nero_index();
+
+/**
+ * Decrements the nero index int the order of nero_menu_t which will be used to select a drive mode
+ */
+int decrement_nero_index();
+
+/**
+ * Transitions out of home mode, therefore into a drive mode based on the
+ * current nero index and mapped using the map_nero_index_to_drive_state function
+ * Wil set the functional state to active
+ */
+int select_nero_index();
+
+/**
+ * Sets the home mode to be true, which will turn off the car and set the functional state to ready
+ */
+int set_home_mode();
+
+/**
+ * Handles a fault, setting the functional state to FAULTED
+ */
+int fault();
+
 #endif
diff --git a/Core/Src/fault.c b/Core/Src/fault.c
@@ -24,16 +24,13 @@ int queue_fault(fault_data_t *fault_data)
 	if (!fault_handle_queue)
 		return -1;
 
-	osMessageQueuePut(fault_handle_queue, fault_data, 0U, 0U);
-	return 0;
+	return osMessageQueuePut(fault_handle_queue, fault_data, 0U, 0U);
 }
 
 void vFaultHandler(void *pv_params)
 {
 	fault_data_t fault_data;
 	osStatus_t status;
-	const state_req_t fault_request = { .id = FUNCTIONAL,
-					    .state.functional = FAULTED };
 	fault_handle_queue = osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE,
 					       sizeof(fault_data_t), NULL);
 
@@ -58,13 +55,13 @@ void vFaultHandler(void *pv_params)
 			       fault_data.diag);
 			switch (fault_data.severity) {
 			case DEFCON1: /* Highest(1st) Priority */
-				queue_state_transition(fault_request);
+				fault();
 				break;
 			case DEFCON2:
-				queue_state_transition(fault_request);
+				fault();
 				break;
 			case DEFCON3:
-				queue_state_transition(fault_request);
+				fault();
 				break;
 			case DEFCON4:
 				break;

diff --git a/Core/Src/nero.c b/Core/Src/nero.c
@@ -10,131 +10,26 @@
 #include "cerberus_conf.h"
 #include "monitor.h"
 
-static nero_state_t nero_state = { .nero_index = 0, .home_mode = true };
 static int8_t mph = 0;
 
 static void send_mode_status()
 {
 	can_msg_t msg = { .id = 0x501,
 			  .len = 4,
-			  .data = { nero_state.home_mode, nero_state.nero_index,
-				    mph, get_tsms() } };
+			  .data = { get_nero_state().home_mode,
+				    get_nero_state().nero_index, mph,
+				    get_tsms() } };
 
 	/* Send CAN message */
 	queue_can_msg(msg);
 }
 
-static drive_state_t map_nero_index_to_drive_state(nero_menu_t nero_index)
-{
-	switch (nero_index) {
-	case OFF:
-		return NOT_DRIVING;
-	case PIT:
-		return SPEED_LIMITED;
-	case PERFORMANCE:
-		return AUTOCROSS;
-	case EFFICIENCY:
-		return ENDURANCE;
-	default:
-		return OFF;
-	}
-}
-
-void increment_nero_index()
-{
-	if (!nero_state.home_mode) {
-		// Do Nothing because we are not in home mode and therefore not tracking the nero index
-		return;
-	}
-
-	if (nero_state.nero_index + 1 < MAX_NERO_STATES) {
-		nero_state.nero_index += 1;
-	} else {
-		// Do Nothing because theres no additional states or we dont care about the additional states;
-	}
-}
-
 void set_mph(int8_t new_mph)
 {
 	//printf("mph %d", new_mph);
 	mph = new_mph;
 }
 
-void decrement_nero_index()
-{
-	serial_print("decrementing with mode, %d", nero_state.home_mode);
-	if (!nero_state.home_mode) {
-		// Do Nothing because we are not in home mode and therefore not tracking the nero index
-		return;
-	}
-
-	if (nero_state.nero_index - 1 >= 0) {
-		nero_state.nero_index -= 1;
-	} else {
-		// Do Nothing because theres no negative states
-	}
-}
-
-void select_nero_index()
-{
-	state_req_t state_request;
-
-	if (!nero_state.home_mode) {
-		// Only Check if we are in pit mode to toggle direction
-		if (nero_state.nero_index == PIT) {
-			if (get_drive_state() == REVERSE) {
-				state_request.id = DRIVE;
-				state_request.state.drive = SPEED_LIMITED;
-				queue_state_transition(state_request);
-			} else {
-				state_request.id = DRIVE;
-				state_request.state.drive = REVERSE;
-				queue_state_transition(state_request);
-			}
-		}
-		return;
-	}
-
-	uint8_t max_drive_states =
-		MAX_DRIVE_STATES -
-		1; // Account for reverse and pit being the same screen
-
-	if (nero_state.nero_index > 0 &&
-	    nero_state.nero_index < max_drive_states && get_tsms() &&
-	    get_brake_state()) {
-		state_request.id = FUNCTIONAL;
-		state_request.state.functional = ACTIVE;
-		queue_state_transition(state_request);
-		state_request.id = DRIVE;
-		state_request.state.drive =
-			map_nero_index_to_drive_state(nero_state.nero_index);
-		queue_state_transition(state_request);
-		nero_state.home_mode = false;
-	} else if (nero_state.nero_index == OFF) {
-		state_request.id = FUNCTIONAL;
-		state_request.state.functional = READY;
-		queue_state_transition(state_request);
-		nero_state.home_mode = false;
-	} else if (nero_state.nero_index >= max_drive_states) {
-		nero_state.home_mode = false;
-	}
-}
-
-void set_nero_home_mode()
-{
-	nero_state.home_mode = true;
-}
-
-void set_home_mode()
-{
-	state_req_t state_request = { .id = FUNCTIONAL,
-				      .state.functional = READY };
-	if (!get_tsms()) {
-		nero_state.home_mode = true;
-		queue_state_transition(state_request);
-	}
-}
-
 osThreadId_t nero_monitor_handle;
 const osThreadAttr_t nero_monitor_attributes = {
 	.name = "NeroMonitor",