Revamping state machine implementation

CM7/Core/Inc/foc_ctrl.h

@@ -8,6 +8,7 @@
 #include "ipark.h"
 #include "svgen.h"
 #include "pid.h"
+#include "state_machine.h"
 
 typedef enum {
     SECTOR_1 = 0,
@@ -61,9 +62,11 @@ typedef struct {
 	PARK_Obj park_transform;
 	IPARK_Obj ipark_transform;
 	SVGEN_Obj svm;
+
+	state_machine_t *sm;
 } foc_ctrl_t;
 
-void foc_ctrl_init(foc_ctrl_t *controller);
+void foc_ctrl_init(foc_ctrl_t *controller, state_machine_t *sm);
 
 /* Enqueue a single frame of controller observation */
 osStatus_t foc_queue_frame(foc_ctrl_t *controller, foc_data_t *phase_currents);

CM7/Core/Inc/state_machine.h

@@ -8,17 +8,21 @@ typedef struct {
     state_t current_state;
     osMutexId_t* state_mutex;
     osMutexAttr_t state_mutex_attr;
-} state_director_t;
+    osMessageQueueId_t state_trans_queue;
+    osThreadId_t thread;
+} state_machine_t;
 
-extern osThreadId_t sm_director_handle;
 extern const osThreadAttr_t sm_director_attributes;
 
-void vStateMachineDirector(void *pv_params);
+/* Initialize a State Machine */
+void state_machine_init(state_machine_t *sm);
 
 /* Adds a functional state transition to be processed */
-int queue_state(state_t new_state);
+int state_machine_queue_state(state_machine_t *sm, state_t new_state);
 
 /* Retrieves the current functional state */
-state_t get_state();
+state_t state_machine_get_state(state_machine_t *sm);
+
+void vStateMachineDirector(void *pv_params);
 
 #endif

CM7/Core/Src/foc_ctrl.c

@@ -29,8 +29,10 @@ enum {
 	Q
 };
 
-void foc_ctrl_init(foc_ctrl_t *controller)
+void foc_ctrl_init(foc_ctrl_t *controller, state_machine_t *sm)
 {
+	assert(sm);
+
 	controller->data_queue = osMessageQueueNew(INBOUND_QUEUE_SIZE, sizeof(foc_data_t), NULL);
 	controller->command_queue = osMessageQueueNew(OUTBOUND_QUEUE_SIZE, sizeof(pwm_signal_t[3]), NULL);
 
@@ -49,6 +51,8 @@ void foc_ctrl_init(foc_ctrl_t *controller)
 
 	controller->last_run_us = us_timer_get();
 	controller->open_loop_ramp_velocity = 0.3;
+
+	controller->sm = sm;
 }
 
 osStatus_t foc_queue_frame(foc_ctrl_t *controller, foc_data_t *data)
@@ -185,7 +189,7 @@ void vFOCctrl(void *pv_params)
 		/* Wait until a message is in the queue, send messages when they are in the queue */
 		status = osMessageQueueGet(controller->data_queue, &msg, NULL, osWaitForever);
 		if (status == osOK) {
-			switch (get_state()) {
+			switch (state_machine_get_state(controller->sm)) {
 				case START:
 					/* Open Loop Startup Procedure */
 					open_loop_ctrl(controller);
@@ -220,6 +224,7 @@ void vFOCctrl(void *pv_params)
 			//uint32_t first_time = us_timer_get();
 			//closed_loop_ctrl(controller, &msg);
 			open_loop_ctrl(controller);
+			printf("AHH\r\n");
 			//printf("Time us:%ld\r\n", us_timer_get() - first_time);
 		}
 

CM7/Core/Src/gatedriver.c

@@ -130,6 +130,7 @@ void vPhaseActor(void *pv_params)
 		status = osMessageQueueGet(gatedriver->controller->command_queue, &duty_cycles, NULL, osWaitForever);
 		if (status == osOK) {
 			gatedrv_write_pwm(gatedriver, duty_cycles);
+			printf("WRITING\r\n");
 		}
 	}
 }

CM7/Core/Src/main.c

@@ -28,6 +28,7 @@
 #include "ssi_encoder.h"
 #include "foc_ctrl.h"
 #include "ipcc.h"
+#include "state_machine.h"
 #include <stdio.h>
 #include <assert.h>
 /* USER CODE END Includes */
@@ -83,13 +84,18 @@ const osThreadAttr_t defaultTask_attributes = {
 };
 /* USER CODE BEGIN PV */
 
+/* Struct for intercore comms */
 ipcc_t ipcc;
 
+/* Control params for left motor */
+state_machine_t sm_left;
 gatedriver_t gatedrv_left;
-gatedriver_t gatedrv_right;
+foc_ctrl_t ctrl_left;
 
+/* Control params for right motor */
+state_machine_t sm_right;
+gatedriver_t gatedrv_right;
 foc_ctrl_t ctrl_right;
-foc_ctrl_t ctrl_left;
 
 /* USER CODE END PV */
 
@@ -250,14 +256,19 @@ int main(void)
   /* USER CODE BEGIN RTOS_MUTEX */
   // lmao we aren't adding mutexes here but need to init stuff after the kernel initializes
   printf("Booting...\r\n");
-  foc_ctrl_init(&ctrl_left);
-  foc_ctrl_init(&ctrl_right);
 
-  gatedrv_init(&gatedrv_left, &htim1, &hadc1, &ctrl_left);
-  //ssi_encoder_t *ssi_encoder_left = ssi_encoder_init(&hspi2);
+  state_machine_init(&sm_left);
+  state_machine_init(&sm_right);
+
+  foc_ctrl_init(&ctrl_left, &sm_left);
+  foc_ctrl_init(&ctrl_right, &sm_right);
 
+  gatedrv_init(&gatedrv_left, &htim1, &hadc1, &ctrl_left);
   gatedrv_init(&gatedrv_right, &htim8, &hadc3, &ctrl_right);
+
+  //ssi_encoder_t *ssi_encoder_left = ssi_encoder_init(&hspi2);
   //ssi_encoder_t *ssi_encoder_right = ssi_encoder_init(&hspi4);
+
   printf("MC Initialized...\r\n");
   /* USER CODE END RTOS_MUTEX */
 
@@ -278,17 +289,21 @@ int main(void)
   defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
 
   /* USER CODE BEGIN RTOS_THREADS */
+  /* Initialize Left Motor Threads */
   gatedrv_left.write_thread = osThreadNew(vPhaseActor, &gatedrv_left, &phase_actor_attributes);
   assert(gatedrv_left.write_thread);
-
   ctrl_left.thread = osThreadNew(vFOCctrl, &ctrl_left, &foc_ctrl_attributes);
   assert(ctrl_left.thread);
+  sm_left.thread = osThreadNew(vStateMachineDirector, &sm_left, &sm_director_attributes);
+  assert(sm_left.thread);
 
+  /* Initialize Right Motor Threads */
   gatedrv_right.write_thread = osThreadNew(vPhaseActor, &gatedrv_right, &phase_actor_attributes);
   assert(gatedrv_right.write_thread);
-
   ctrl_right.thread = osThreadNew(vFOCctrl, &ctrl_right, &foc_ctrl_attributes);
   assert(ctrl_right.thread);
+  sm_right.thread = osThreadNew(vStateMachineDirector, &sm_right, &sm_director_attributes);
+  assert(sm_right.thread);
 
   printf("Tasks Created...\r\n");
   /* USER CODE END RTOS_THREADS */
@@ -1388,25 +1403,13 @@ void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
   int curr_time = us_timer_get();
-  float duty_cycles[3] = {0.5, 0.5, 0.5};
-  gatedrv_write_pwm(&gatedrv_left, duty_cycles);
-  gatedrv_write_pwm(&gatedrv_right, duty_cycles);
   ipcc_msg_t msg = {.id = IPCC_FAULT_ALERT, .msg.fault = 0};
   /* Infinite loop */
   for (;;)
   {
     HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
     osDelay(500);
-    printf("U: %ld A, V: %ld A, W: %ld A, Time: %ld us\r\n",
-           (uint32_t)(duty_cycles[0] * 100),
-           (uint32_t)(duty_cycles[1] * 100),
-           (uint32_t)(duty_cycles[2] * 100),
-           us_timer_get() - curr_time);
-    duty_cycles[0] = duty_cycles[0] + 0.01 >= 1.0 ? 0.0 : duty_cycles[0] + 0.01;
-    duty_cycles[1] = duty_cycles[1] + 0.01 >= 1.0 ? 0.0 : duty_cycles[1] + 0.01;
-    duty_cycles[2] = duty_cycles[2] + 0.01 >= 1.0 ? 0.0 : duty_cycles[2] + 0.01;
-    gatedrv_write_pwm(&gatedrv_left, duty_cycles);
-    gatedrv_write_pwm(&gatedrv_right, duty_cycles);
+    //printf("Time: %ld us\r\n", us_timer_get() - curr_time);
     curr_time = us_timer_get();
     //printf("Failure: %d\r\n", ipcc_transfer(&ipcc, &msg));
   }

CM7/Core/Src/state_machine.c

@@ -5,9 +5,6 @@
 
 #define STATE_TRANS_QUEUE_SIZE 16
 
-/* Internal State of Vehicle */
-state_director_t proteus_state;
-
 /* State Transition Map */
 // TODO: fill out state transition map
 static const bool valid_trans_to_from[MAX_FUNC_STATES][MAX_FUNC_STATES] = {
@@ -24,52 +21,55 @@ static const bool valid_trans_to_from[MAX_FUNC_STATES][MAX_FUNC_STATES] = {
     {true, true, false, false, false, false, false, false, true, true},    /* FAULTED */
 };
 
-osThreadId_t sm_director_handle;
-const osThreadAttr_t sm_director_attributes = {
-    .name = "State Machine Director",
-    .stack_size = 128 * 4,
-    .priority = (osPriority_t)osPriorityAboveNormal3,
-};
+void state_machine_init(state_machine_t *sm)
+{
+    sm->current_state = LV_BOOT;
 
-static osMessageQueueId_t state_trans_queue;
+    sm->state_mutex = osMutexNew(&sm->state_mutex_attr);
+	assert(sm->state_mutex);
+
+    sm->state_trans_queue = osMessageQueueNew(STATE_TRANS_QUEUE_SIZE, sizeof(state_t), NULL);
+}
 
-int queue_state(state_t new_state)
+int state_machine_queue_state(state_machine_t *sm, state_t new_state)
 {
-    if (!state_trans_queue)
+    if (!sm->state_trans_queue)
         return 1;
 
-    return osMessageQueuePut(state_trans_queue, &new_state, 0U, 0U);
+    return osMessageQueuePut(sm->state_trans_queue, &new_state, 0U, 0U);
 }
 
-state_t get_state()
+state_t state_machine_get_state(state_machine_t *sm)
 {
     state_t ret;
-    osStatus_t mut_stat = osMutexAcquire(proteus_state.state_mutex, osWaitForever);
+    osStatus_t mut_stat = osMutexAcquire(sm->state_mutex, osWaitForever);
 	if (mut_stat) {
 		return -1;
 	}
 
-    ret = proteus_state.current_state;
+    ret = sm->current_state;
 
-    osMutexRelease(proteus_state.state_mutex);
+    osMutexRelease(sm->state_mutex);
 
     return ret;
 }
 
+const osThreadAttr_t sm_director_attributes = {
+    .name = "State Machine Director",
+    .stack_size = 128 * 4,
+    .priority = (osPriority_t)osPriorityAboveNormal3,
+};
+
 void vStateMachineDirector(void *pv_params)
 {
-    proteus_state.current_state = LV_BOOT;
-
-    proteus_state.state_mutex = osMutexNew(&proteus_state.state_mutex_attr);
-	assert(proteus_state.state_mutex);
-
-    state_trans_queue = osMessageQueueNew(STATE_TRANS_QUEUE_SIZE, sizeof(state_t), NULL);
+    state_machine_t *sm = (state_machine_t *)pv_params;
+	assert(sm);
 
     state_t new_state;
 
     for (;;)
     {
-        if (osOK != osMessageQueueGet(state_trans_queue, &new_state, NULL, 50))
+        if (osOK != osMessageQueueGet(sm->state_trans_queue, &new_state, NULL, 50))
         {
             // TODO queue fault, low criticality
             continue;