diff --git a/Core/Inc/nero.h b/Core/Inc/nero.h
index 75f5754..e0d2771 100644
--- 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
index 6335e40..10648a1 100644
--- 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
index cd59db0..d29f883 100644
--- 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
\ No newline at end of file
diff --git a/Core/Src/fault.c b/Core/Src/fault.c
index 28d4739..944b592 100644
--- 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
index b3df8e8..f6a3c4e 100644
--- 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",
diff --git a/Core/Src/state_machine.c b/Core/Src/state_machine.c
index fda25b1..b279e2c 100644
--- a/Core/Src/state_machine.c
+++ b/Core/Src/state_machine.c
@@ -4,6 +4,7 @@
 #include "nero.h"
 #include "pdu.h"
 #include "queues.h"
+#include "monitor.h"
 #include "serial_monitor.h"
 #include "nero.h"
 #include "pdu.h"
@@ -13,11 +14,6 @@
 
 #define STATE_TRANS_QUEUE_SIZE 4
 
-typedef struct {
-	func_state_t functional;
-	drive_state_t drive;
-} state_t;
-
 /* Internal State of Vehicle */
 static state_t cerberus_state;
 
@@ -27,6 +23,14 @@ extern IWDG_HandleTypeDef hiwdg;
 
 osTimerId fault_timer;
 
+typedef struct {
+	enum { FUNCTIONAL, NERO } id;
+	union {
+		func_state_t functional;
+		nero_state_t nero;
+	} state;
+} state_req_t;
+
 /* State Transition Map */
 static const bool valid_trans_to_from[MAX_FUNC_STATES][MAX_FUNC_STATES] = {
 	/*BOOT  READY DRIVING FAULTED*/
@@ -44,28 +48,45 @@ const osThreadAttr_t sm_director_attributes = {
 };
 
 static osMessageQueueId_t state_trans_queue;
-osMessageQueueId_t break_state_queue;
 
-void fault_callback()
+func_state_t get_func_state()
 {
-	state_req_t req = { .id = FUNCTIONAL, .state.functional = READY };
-	queue_state_transition(req);
+	return cerberus_state.functional;
 }
 
-int queue_state_transition(state_req_t new_state)
+drive_state_t get_drive_state()
 {
-	if (!state_trans_queue) {
-		return 1;
-	}
+	return cerberus_state.drive;
+}
 
-	printf("QUEUING STATAE TRANSITION\r\n");
+nero_state_t get_nero_state()
+{
+	return cerberus_state.nero;
+}
 
-	if (new_state.id == DRIVE) {
-		if (get_func_state() != ACTIVE)
-			return 0;
+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;
+	}
+}
 
-		/* Transitioning between drive states is always allowed */
-		// TODO: Make sure motor is not spinning before switching
+static int transition_drive_state(drive_state_t new_state)
+{
+	if (get_func_state() != ACTIVE)
+		return 0;
+
+	/* Transitioning between drive states is always allowed */
+	// TODO: Make sure motor is not spinning before switching
 
 		/* If we are turning ON the motor, blare RTDS */
 		if (cerberus_state.drive == NOT_DRIVING) {
@@ -75,171 +96,199 @@ int queue_state_transition(state_req_t new_state)
 			sound_rtds(pdu);
 		}
 
-		cerberus_state.drive = new_state.state.drive;
-		return 0;
+	cerberus_state.drive = new_state;
+	return 0;
+}
+
+static int transition_functional_state(func_state_t new_state)
+{
+	if (!valid_trans_to_from[cerberus_state.functional][new_state]) {
+		printf("Invalid State transition");
+		return -1;
+	}
+
+	/* Catching state transitions */
+	switch (new_state) {
+	case BOOT:
+		/* Do Nothing */
+		break;
+	case READY:
+		/* Turn off high power peripherals */
+		// write_fan_battbox(pdu, false);
+		write_pump(pdu, false);
+		write_fault(pdu, true);
+		transition_drive_state(OFF);
+		printf("READY\r\n");
+		break;
+	case ACTIVE:
+		serial_print("BRAKE: %d\n", get_brake_state());
+		if (!get_tsms() || !get_brake_state())
+			return 1; // Cannot go into active if tsms is off
+		/* Turn on high power peripherals */
+		// write_fan_battbox(pdu, true);
+		write_pump(pdu, true);
+		write_fault(pdu, true);
+		sound_rtds(pdu); // TEMPORARY
+		printf("ACTIVE STATE\r\n");
+		break;
+	case FAULTED:
+		/* Turn off high power peripherals */
+		// write_fan_battbox(pdu, true);
+		write_pump(pdu, false);
+		write_fault(pdu, false);
+		// DO NOT CHANGE WITHOUT CHECKING the bms fault timer, as if BMS timer is later could result in a fault/unfault/fault flicker.
+		osTimerStart(fault_timer, 5000);
+		HAL_IWDG_Refresh(&hiwdg);
+		transition_drive_state(OFF);
+		cerberus_state.nero =
+			(nero_state_t){ .nero_index = OFF, .home_mode = true };
+		printf("FAULTED\r\n");
+		break;
+	default:
+		// Do Nothing
+		break;
 	}
 
-	if (new_state.id == FUNCTIONAL) {
-		if (!valid_trans_to_from[cerberus_state.functional]
-					[new_state.state.functional]) {
-			printf("Invalid State transition");
-			return -1;
+	cerberus_state.functional = new_state;
+	return 0;
+}
+
+static int transition_nero_state(nero_state_t new_state)
+{
+	nero_state_t current_nero_state = get_nero_state();
+
+	// If we are not in home mode, we should not change the nero index
+	if (!new_state.home_mode)
+		new_state.nero_index = current_nero_state.nero_index;
+
+	// Checks for when we are in home mode and the nero index is out of bounds
+	if (new_state.nero_index < 0)
+		new_state.nero_index = 0;
+	if (new_state.nero_index >= MAX_NERO_STATES)
+		new_state.nero_index = MAX_NERO_STATES - 1;
+
+	// Wasn't in home mode and still are not in home mode (Infer a Select Request)
+	if (!current_nero_state.home_mode && !new_state.home_mode) {
+		// Only Check if we are in pit mode to toggle direction
+		if (current_nero_state.nero_index == PIT) {
+			if (get_drive_state() == REVERSE) {
+				if (transition_drive_state(SPEED_LIMITED))
+					return 1;
+			} else {
+				if (transition_drive_state(REVERSE))
+					return 1;
+			}
 		}
+	}
 
-		cerberus_state.functional = new_state.state.functional;
-		/* Catching state transitions */
-		switch (new_state.state.functional) {
-		case BOOT:
-			/* Do Nothing */
-			break;
-		case READY:
-			/* Turn off high power peripherals */
-			serial_print("going to ready");
-			//write_fan_battbox(pdu, false);
-			write_pump(pdu, false);
-			write_fault(pdu, true);
-			cerberus_state.drive = OFF;
-			printf("READY\r\n");
-			break;
-		case ACTIVE:
-			/* Turn on high power peripherals */
-			//write_fan_battbox(pdu, true);
-			write_pump(pdu, true);
-			write_fault(pdu, true);
-			sound_rtds(pdu); // TEMPORARY
-			printf("ACTIVE STATE\r\n");
-			break;
-		case FAULTED:
-			/* Turn off high power peripherals */
-			//write_fan_battbox(pdu, true);
-			write_pump(pdu, false);
-			write_fault(pdu, false);
-			// DO NOT CHANGE WITHOUT CHECKING the bms fault timer, as if BMS timer is later could result in a fault/unfault/fault flicker.
-			osTimerStart(fault_timer, 5000);
-			HAL_IWDG_Refresh(&hiwdg);
-			cerberus_state.drive = OFF;
-			set_nero_home_mode();
-			printf("FAULTED\r\n");
-			break;
-		default:
-			// Do Nothing
-			break;
+	uint8_t max_drive_states =
+		MAX_DRIVE_STATES -
+		1; // Account for reverse and pit being the same screen
+
+	serial_print("NEW HOME MODE, %d \n", new_state.home_mode);
+	// Selecting a mode on NERO
+	if (current_nero_state.home_mode && !new_state.home_mode) {
+		if (new_state.nero_index > 0 &&
+		    new_state.nero_index < max_drive_states) {
+			if (transition_functional_state(ACTIVE))
+				return 1;
+			if (transition_drive_state(
+				    map_nero_index_to_drive_state(
+					    new_state.nero_index)))
+				return 1;
+		} else if (new_state.nero_index == OFF) {
+			if (transition_functional_state(READY))
+				return 1;
 		}
-		return 0;
 	}
 
+	// Entering home mode
+	if (!current_nero_state.home_mode && new_state.home_mode) {
+		if (transition_functional_state(READY))
+			return 1;
+	}
+
+	cerberus_state.nero = new_state;
 	return 0;
 }
 
-func_state_t get_func_state()
+static int queue_state_transition(state_req_t new_state)
 {
-	return cerberus_state.functional;
+	if (!state_trans_queue) {
+		return 1;
+	}
+
+	return osMessageQueuePut(state_trans_queue, &new_state, 0U, 0U);
 }
 
-drive_state_t get_drive_state()
+/* HANDLE USER INPUT */
+int increment_nero_index()
 {
-	return cerberus_state.drive;
+	return queue_state_transition((state_req_t){
+		.id = NERO,
+		.state.nero = (nero_state_t){
+			.home_mode = get_nero_state().home_mode,
+			.nero_index = get_nero_state().nero_index + 1 } });
+}
+
+int decrement_nero_index()
+{
+	return queue_state_transition((state_req_t){
+		.id = NERO,
+		.state.nero = (nero_state_t){
+			.home_mode = get_nero_state().home_mode,
+			.nero_index = get_nero_state().nero_index - 1 } });
+}
+
+int select_nero_index()
+{
+	return queue_state_transition((state_req_t){
+		.id = NERO,
+		.state.nero = (nero_state_t){
+			.home_mode = false,
+			.nero_index = get_nero_state().nero_index } });
+}
+
+int set_home_mode()
+{
+	return queue_state_transition((state_req_t){
+		.id = NERO,
+		.state.nero = { .nero_index = get_nero_state().nero_index,
+				.home_mode = true } });
+}
+
+/* HANDLE FAULTS */
+int fault()
+{
+	return queue_state_transition(
+		(state_req_t){ .id = FUNCTIONAL, .state.functional = FAULTED });
 }
 
 void vStateMachineDirector(void *pv_params)
 {
-	cerberus_state.functional = BOOT;
+	cerberus_state.functional = READY;
 	cerberus_state.drive = NOT_DRIVING;
+	cerberus_state.nero.nero_index = 0;
+	cerberus_state.nero.home_mode = true;
 
 	state_trans_queue = osMessageQueueNew(STATE_TRANS_QUEUE_SIZE,
 					      sizeof(state_req_t), NULL);
-	break_state_queue = osMessageQueueNew(1, sizeof(bool), NULL);
 
 	pdu_t *pdu_1 = (pdu_t *)pv_params;
 
 	pdu = pdu_1;
-	state_req_t new_state;
+	state_req_t new_state_req;
 
 	serial_print("State Machine Init!\r\n");
 
-	// debug bootup message
-	can_msg_t bootup_msg = { .id = 0x701,
-				 .len = 8,
-				 .data = { 0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
-					   0xFF, 0xFF } };
-	queue_can_msg(bootup_msg);
-
-	state_req_t request;
-	request.id = FUNCTIONAL;
-	request.state.functional = READY;
-	queue_state_transition(request);
-
-	fault_timer = osTimerNew(&fault_callback, osTimerOnce, NULL, NULL);
-
 	for (;;) {
-		osMessageQueueGet(state_trans_queue, &new_state, NULL,
+		osMessageQueueGet(state_trans_queue, &new_state_req, NULL,
 				  osWaitForever);
 
-		if (new_state.id == DRIVE) {
-			if (get_func_state() != ACTIVE)
-				continue;
-
-			/* Transitioning between drive states is always allowed */
-			// TODO: Make sure motor is not spinning before switching
-
-			/* If we are turning ON the motor, blare RTDS */
-			if (cerberus_state.drive == NOT_DRIVING) {
-				/* make sure foot is on break */
-				bool break_state = false;
-				//osMessageQueueGet(break_state_queue, &break_state, NULL, 0);
-				if (!break_state) {
-					continue;
-				}
-
-				serial_print("CALLING RTDS");
-				sound_rtds(pdu);
-			}
-
-			cerberus_state.drive = new_state.state.drive;
-			continue;
-		}
-
-		if (new_state.id == FUNCTIONAL) {
-			if (!valid_trans_to_from[cerberus_state.functional]
-						[new_state.state.functional]) {
-				printf("Invalid State transition");
-				continue;
-			}
-
-			cerberus_state.functional = new_state.state.functional;
-			/* Catching state transitions */
-			switch (new_state.state.functional) {
-			case BOOT:
-				/* Do Nothing */
-				break;
-			case READY:
-				/* Turn off high power peripherals */
-				serial_print("going to ready");
-				//write_fan_battbox(pdu, false);
-				write_pump(pdu, true);
-				write_fault(pdu, true);
-				break;
-			case ACTIVE:
-				/* Turn on high power peripherals */
-				//write_fan_battbox(pdu, true);
-				write_pump(pdu, true);
-				write_fault(pdu, true);
-				break;
-			case FAULTED:
-				/* Turn off high power peripherals */
-				//write_fan_battbox(pdu, false);
-				write_pump(pdu, false);
-				write_fault(pdu, false);
-				HAL_IWDG_Refresh(&hiwdg);
-				while (1) {
-					printf("Delaying...\r\n");
-				}
-				break;
-			default:
-				// Do Nothing
-				break;
-			}
-			continue;
-		}
+		if (new_state_req.id == NERO)
+			transition_nero_state(new_state_req.state.nero);
+		else if (new_state_req.id == FUNCTIONAL)
+			transition_functional_state(
+				new_state_req.state.functional);
 	}
 }