Merge pull request #216 from Northeastern-Electric-Racing/feature/fau…

…lt-clearing

Feature/fault clearing

Core/Src/state_machine.c

@@ -19,6 +19,9 @@
 /* Internal State of Vehicle */
 static state_t cerberus_state;
 
+/* Timer to unfault */
+static osTimerId_t unfault_timer;
+
 typedef struct {
 	enum { FUNCTIONAL, NERO } id;
 	union {
@@ -100,9 +103,10 @@ static int transition_functional_state(func_state_t new_state, pdu_t *pdu,
 		/* Turn off high power peripherals */
 		// write_fan_battbox(pdu, true);
 		write_pump(pdu, false);
-		write_fault(pdu, true);
 		cerberus_state.nero =
 			(nero_state_t){ .nero_index = OFF, .home_mode = false };
+		osDelay(1000); /* Delay for 1 sec before faulting car */
+		write_fault(pdu, true);
 		serial_print("FAULTED\r\n");
 		break;
 	default:
@@ -222,10 +226,19 @@ int set_home_mode()
 
 int fault()
 {
+	// count 5 seconds before unfaulting
+	osTimerStart(unfault_timer, pdMS_TO_TICKS(5 * 1000));
 	return queue_state_transition(
 		(state_req_t){ .id = FUNCTIONAL, .state.functional = FAULTED });
 }
 
+void unfault_timer_callback(void *args)
+{
+	printf("UNFAULTING");
+	queue_state_transition(
+		(state_req_t){ .id = FUNCTIONAL, .state.functional = READY });
+}
+
 void vStateMachineDirector(void *pv_params)
 {
 	cerberus_state.functional = READY;
@@ -242,6 +255,9 @@ void vStateMachineDirector(void *pv_params)
 	dti_t *mc = args->mc;
 	free(args);
 
+	unfault_timer =
+		osTimerNew(unfault_timer_callback, osTimerOnce, NULL, NULL);
+
 	/* Write to GPIO expander to set initial state */
 	write_pump(pdu, false);
 	write_fault(pdu, false);