diff --git a/fsae-pcc/src/can.cpp b/fsae-pcc/src/can.cpp
index b2fc4d1..26788f5 100644
--- a/fsae-pcc/src/can.cpp
+++ b/fsae-pcc/src/can.cpp
@@ -11,11 +11,11 @@
 #define PCC_CAN_ID 0x123
 
 typedef struct __attribute__((packed)) {
-    uint32_t timestamp; // Timestamp in milliseconds
-    uint8_t state;      // Precharge state
+    // uint32_t timestamp; // Timestamp in milliseconds
+    uint8_t state;     // Precharge state
     uint8_t errorCode; // Error code
-    float accumulatorVoltage; // Accumulator voltage in volts
-    float tsVoltage; // Transmission side voltage in volts
+    // float accumulatorVoltage; // Accumulator voltage in volts
+    // float tsVoltage; // Transmission side voltage in volts
     float prechargeProgress; // Precharge progress in percent
 } PCC;
 
@@ -34,14 +34,16 @@ void CAN_Init() {
     pccMsg.id = PCC_CAN_ID; // can change ID
 }
 
-void CAN_SendPCCMessage(uint32_t timestamp, uint8_t state, uint8_t errorCode, float accumulatorVoltage, float tsVoltage, float prechargeProgress) {
+void CAN_SendPCCMessage(uint32_t timestamp, uint8_t state, uint8_t errorCode,
+                        float accumulatorVoltage, float tsVoltage,
+                        float prechargeProgress) {
     pccData = {
-        .timestamp = timestamp,
-        .state = state,
-        .errorCode = errorCode,
-        .accumulatorVoltage = accumulatorVoltage,
-        .tsVoltage = tsVoltage,
-        .prechargeProgress = prechargeProgress
+        //.timestamp = timestamp, 4 bytes
+        .state = state,         // 1 bytes
+        .errorCode = errorCode, // 1 bytes
+        //.accumulatorVoltage = accumulatorVoltage, 4 bytes
+        //.tsVoltage = tsVoltage, 4 bytes
+        .prechargeProgress = prechargeProgress // 4 bytes
     };
 
     // Serial.print("PCC Message: ");
@@ -60,4 +62,4 @@ void CAN_SendPCCMessage(uint32_t timestamp, uint8_t state, uint8_t errorCode, fl
 
     memcpy(pccMsg.buf, &pccData, sizeof(PCC));
     can1.write(pccMsg);
-}
\ No newline at end of file
+}
diff --git a/fsae-pcc/src/can.h b/fsae-pcc/src/can.h
index 1ebff2a..c08f824 100644
--- a/fsae-pcc/src/can.h
+++ b/fsae-pcc/src/can.h
@@ -5,4 +5,6 @@
 #include <stdint.h>
 
 void CAN_Init();
-void CAN_SendPCCMessage(uint32_t timestamp, uint8_t state, uint8_t errorCode, float accumulatorVoltage, float tsVoltage, float prechargeProgress);
\ No newline at end of file
+void CAN_SendPCCMessage(uint32_t timestamp, uint8_t state, uint8_t errorCode,
+                        float accumulatorVoltage, float tsVoltage,
+                        float prechargeProgress);
diff --git a/fsae-pcc/src/gpio.cpp b/fsae-pcc/src/gpio.cpp
index d0533f4..dad0059 100644
--- a/fsae-pcc/src/gpio.cpp
+++ b/fsae-pcc/src/gpio.cpp
@@ -1,11 +1,11 @@
 // Anteater Electric Racing, 2025
 
-#include <Arduino.h>
 #include "gpio.h"
 #include "utils.h"
+#include <Arduino.h>
 
-void gpioInit(void){
+void gpioInit(void) {
     pinMode(SHUTDOWN_CTRL_PIN, INPUT);
     pinMode(FREQ_ACCU_PIN, INPUT);
     pinMode(FREQ_TS_PIN, INPUT);
-}
\ No newline at end of file
+}
diff --git a/fsae-pcc/src/gpio.h b/fsae-pcc/src/gpio.h
index 9931ddb..220f717 100644
--- a/fsae-pcc/src/gpio.h
+++ b/fsae-pcc/src/gpio.h
@@ -2,4 +2,4 @@
 
 #pragma once
 
-void gpioInit(void);
\ No newline at end of file
+void gpioInit(void);
diff --git a/fsae-pcc/src/main.cpp b/fsae-pcc/src/main.cpp
index 8c8571e..70d61e4 100644
--- a/fsae-pcc/src/main.cpp
+++ b/fsae-pcc/src/main.cpp
@@ -11,17 +11,19 @@
 
 #include <Arduino.h>
 #include <arduino_freertos.h>
-#include "precharge.h"
+
+#include "can.h"
 #include "gpio.h"
+#include "precharge.h"
 #include "utils.h"
-#include "can.h"
 
 static void threadMain(void *pvParameters);
 
 void setup() {
     Serial.begin(9600);
 
-    xTaskCreate(threadMain, "threadMain", THREAD_MAIN_STACK_SIZE, NULL, THREAD_MAIN_PRIORITY, NULL);
+    xTaskCreate(threadMain, "threadMain", THREAD_MAIN_STACK_SIZE, NULL,
+                THREAD_MAIN_PRIORITY, NULL);
 
     gpioInit(); // Initialize GPIO pins
 
@@ -48,21 +50,21 @@ void threadMain(void *pvParameters) {
 
         Serial.print("State: ");
         switch (state) {
-            case STATE_STANDBY:
-                Serial.print("STANDBY");
-                break;
-            case STATE_PRECHARGE:
-                Serial.print("PRECHARGE");
-                break;
-            case STATE_ONLINE:
-                Serial.print("ONLINE");
-                break;
-            case STATE_ERROR:
-                Serial.print("ERROR");
-                break;
-            default:
-                Serial.print("UNDEFINED");
-                break;
+        case STATE_STANDBY:
+            Serial.print("STANDBY");
+            break;
+        case STATE_PRECHARGE:
+            Serial.print("PRECHARGE");
+            break;
+        case STATE_ONLINE:
+            Serial.print("ONLINE");
+            break;
+        case STATE_ERROR:
+            Serial.print("ERROR");
+            break;
+        default:
+            Serial.print("UNDEFINED");
+            break;
         }
         Serial.print(" | Accumulator Voltage: ");
         Serial.print(accumulator_voltage, 4);
@@ -76,5 +78,4 @@ void threadMain(void *pvParameters) {
     }
 }
 
-void loop() {
-}
+void loop() {}
diff --git a/fsae-pcc/src/precharge.cpp b/fsae-pcc/src/precharge.cpp
index 448c43d..fdca64c 100644
--- a/fsae-pcc/src/precharge.cpp
+++ b/fsae-pcc/src/precharge.cpp
@@ -3,10 +3,11 @@
 #include <Arduino.h>
 #include <FreeRTOS.h>
 #include <stdint.h>
-#include "semphr.h"
+
+#include "can.h"
 #include "precharge.h"
+#include "semphr.h"
 #include "utils.h"
-#include "can.h"
 
 #define PRECHARGE_STACK_SIZE 512
 #define PRECHARGE_PRIORITY 1
@@ -25,65 +26,72 @@ static double prechargeProgress = 0.0F;
 typedef struct {
     float tsAlpha;
     float accumAlpha;
-    float filtered_TSF;   // filtered tractive system Frequency
-    float filtered_ACF;   // filtered Accumulator Frequency
+    float filtered_TSF; // filtered tractive system Frequency
+    float filtered_ACF; // filtered Accumulator Frequency
 } LowPassFilter;
 
 static LowPassFilter lpfValues = {0.0F, 0.0F, 0.0F};
 
 static void prechargeTask(void *pvParameters);
 
-float getFrequency(int pin){
+float getFrequency(int pin) {
     const unsigned int TIMEOUT = 700;
-    unsigned int tHigh = pulseIn(pin, 1, TIMEOUT);  // microseconds
+    unsigned int tHigh = pulseIn(pin, 1, TIMEOUT); // microseconds
     unsigned int tLow = pulseIn(pin, 0, TIMEOUT);
-    if (tHigh == 0 || tLow == 0){
+    if (tHigh == 0 || tLow == 0) {
         return 0; // timed out
     }
-    return ( 1000000.0 / (float)(tHigh + tLow) );    // f = 1/T
+    return (1000000.0 / (float)(tHigh + tLow)); // f = 1/T
 }
 
-float getVoltage(int pin){
+float getVoltage(int pin) {
     float rawFreq = getFrequency(pin);
     float voltage = 0.0F;
 
     switch (pin) {
-        case ACCUMULATOR_VOLTAGE_PIN:
-            if (lpfValues.filtered_ACF == 0.0 && rawFreq != 0.0){
-                lpfValues.filtered_ACF = FREQ_TO_VOLTAGE(rawFreq);
-                break;
-            }
-            if(rawFreq == 0.0F) rawFreq = lpfValues.filtered_ACF;
-            LOWPASS_FILTER(rawFreq, lpfValues.filtered_ACF, lpfValues.accumAlpha);
-            voltage = FREQ_TO_VOLTAGE(lpfValues.filtered_ACF); // Convert frequency to voltage
-            break;
-        case TS_VOLTAGE_PIN:
-            if(rawFreq == 0.0F) rawFreq = lpfValues.filtered_TSF;
-            LOWPASS_FILTER(rawFreq, lpfValues.filtered_TSF, lpfValues.tsAlpha);
-            voltage = FREQ_TO_VOLTAGE(lpfValues.filtered_TSF); // Convert frequency to voltage
+    case ACCUMULATOR_VOLTAGE_PIN:
+        if (lpfValues.filtered_ACF == 0.0 && rawFreq != 0.0) {
+            lpfValues.filtered_ACF = FREQ_TO_VOLTAGE(rawFreq);
             break;
-        default:
-            Serial.println("Error: Invalid pin for voltage measurement.");
-            return 0.0F; // Handle error
+        }
+        if (rawFreq == 0.0F)
+            rawFreq = lpfValues.filtered_ACF;
+        LOWPASS_FILTER(rawFreq, lpfValues.filtered_ACF, lpfValues.accumAlpha);
+        voltage = FREQ_TO_VOLTAGE(
+            lpfValues.filtered_ACF); // Convert frequency to voltage
+        break;
+    case TS_VOLTAGE_PIN:
+        if (rawFreq == 0.0F)
+            rawFreq = lpfValues.filtered_TSF;
+        LOWPASS_FILTER(rawFreq, lpfValues.filtered_TSF, lpfValues.tsAlpha);
+        voltage = FREQ_TO_VOLTAGE(
+            lpfValues.filtered_TSF); // Convert frequency to voltage
+        break;
+    default:
+        Serial.println("Error: Invalid pin for voltage measurement.");
+        return 0.0F; // Handle error
     }
 
     return voltage;
 }
 
 // Initialize mutex and precharge task
-void prechargeInit(){
+void prechargeInit() {
 
-    lpfValues.tsAlpha = COMPUTE_ALPHA(100.0F); // 100Hz cutoff frequency for lowpass filter
-    lpfValues.accumAlpha = COMPUTE_ALPHA(1.0F); // 1Hz cutoff frequency for lowpass filter
+    lpfValues.tsAlpha =
+        COMPUTE_ALPHA(100.0F); // 100Hz cutoff frequency for lowpass filter
+    lpfValues.accumAlpha =
+        COMPUTE_ALPHA(1.0F); // 1Hz cutoff frequency for lowpass filter
 
     // Create precharge task
-    xTaskCreate(prechargeTask, "PrechargeTask", PRECHARGE_STACK_SIZE, NULL, PRECHARGE_PRIORITY, NULL);
+    xTaskCreate(prechargeTask, "PrechargeTask", PRECHARGE_STACK_SIZE, NULL,
+                PRECHARGE_PRIORITY, NULL);
 
     Serial.println("Precharge initialized");
 }
 
 // Main precharge task: handles state machine and status updates
-void prechargeTask(void *pvParameters){
+void prechargeTask(void *pvParameters) {
 
     // Stores the last time the last time task was ran
     TickType_t xLastWakeTime;
@@ -92,48 +100,50 @@ void prechargeTask(void *pvParameters){
     // Get current time
     xLastWakeTime = xTaskGetTickCount();
 
-
-    while (true){
-        accVoltage = getVoltage(ACCUMULATOR_VOLTAGE_PIN); // Get raw accumulator voltage
-        tsVoltage = getVoltage(TS_VOLTAGE_PIN); // Get raw tractive system voltage
+    while (true) {
+        accVoltage =
+            getVoltage(ACCUMULATOR_VOLTAGE_PIN); // Get raw accumulator voltage
+        tsVoltage =
+            getVoltage(TS_VOLTAGE_PIN); // Get raw tractive system voltage
 
         // taskENTER_CRITICAL(); // Ensure atomic access to state
-        switch(state){
-            case STATE_STANDBY:
-                standby();
-                break;
-
-            case STATE_PRECHARGE:
-                precharge();
-                break;
+        switch (state) {
+        case STATE_STANDBY:
+            standby();
+            break;
 
-            case STATE_ONLINE:
-                running();
-                break;
+        case STATE_PRECHARGE:
+            precharge();
+            break;
 
-            case STATE_ERROR:
-                errorState();
-                break;
+        case STATE_ONLINE:
+            running();
+            break;
 
-            default: // Undefined state
-                state = STATE_ERROR;
-                errorCode |= ERR_STATE_UNDEFINED;
-                errorState();
+        case STATE_ERROR:
+            errorState();
+            break;
 
+        default: // Undefined state
+            state = STATE_ERROR;
+            errorCode |= ERR_STATE_UNDEFINED;
+            errorState();
         }
         // taskEXIT_CRITICAL(); // Exit critical section
 
         // Send CAN message of current PCC state
-        // CAN_SendPCCMessage(millis(), state, errorCode, accVoltage, tsVoltage, prechargeProgress);
+        // CAN_SendPCCMessage(millis(), state, errorCode, accVoltage, tsVoltage,
+        // prechargeProgress);
 
         // Wait for next cycle
         vTaskDelayUntil(&xLastWakeTime, xFrequency);
     }
 }
 
-// STANDBY STATE: Open AIRs, Open Precharge, indicate status, wait for stable SDC
-void standby(){
-static unsigned long epoch;
+// STANDBY STATE: Open AIRs, Open Precharge, indicate status, wait for stable
+// SDC
+void standby() {
+    static unsigned long epoch;
     if (lastState != STATE_STANDBY) {
         lastState = STATE_STANDBY;
         Serial.println(" === STANDBY ");
@@ -148,15 +158,14 @@ static unsigned long epoch;
     // Disable AIR, Disable Precharge
     digitalWrite(SHUTDOWN_CTRL_PIN, LOW);
 
-    // accVoltage = getVoltage(ACCUMULATOR_VOLTAGE_PIN); // Get raw accumulator voltage
-    // tsVoltage = getVoltage(TS_VOLTAGE_PIN); // Get raw tractive system voltage
-    // Serial.print("Accumulator Voltage: ");
-    // Serial.print(acv);
+    // accVoltage = getVoltage(ACCUMULATOR_VOLTAGE_PIN); // Get raw accumulator
+    // voltage tsVoltage = getVoltage(TS_VOLTAGE_PIN); // Get raw tractive
+    // system voltage Serial.print("Accumulator Voltage: "); Serial.print(acv);
     // Serial.println(" V");
     // Check for stable shutdown circuit
     if (accVoltage >= PCC_MIN_ACC_VOLTAGE) {
-        if (millis() > epoch + PCC_WAIT_TIME){
-        state = STATE_PRECHARGE;
+        if (millis() > epoch + PCC_WAIT_TIME) {
+            state = STATE_PRECHARGE;
         }
     } else {
         epoch = millis(); // reset timer
@@ -164,7 +173,7 @@ static unsigned long epoch;
 }
 
 // PRECHARGE STATE: Close AIR- and precharge relay, monitor precharge voltage
-void precharge(){
+void precharge() {
     unsigned long now = millis();
     static unsigned long epoch;
     static unsigned long lastTimeBelowThreshold;
@@ -173,7 +182,8 @@ void precharge(){
 
     if (lastState != STATE_PRECHARGE) {
         lastState = STATE_PRECHARGE;
-        Serial.printf(" === PRECHARGE   Target precharge %4.1f%%\n", PCC_TARGET_PERCENT);
+        Serial.printf(" === PRECHARGE   Target precharge %4.1f%%\n",
+                      PCC_TARGET_PERCENT);
         epoch = now;
         timePrechargeStart = now;
     }
@@ -195,9 +205,11 @@ void precharge(){
     }
 
     // Check if precharge complete
-    if ( prechargeProgress >= PCC_TARGET_PERCENT ) {
-        if ((now - lastTimeBelowThreshold) > hyst){
-            if (now < timePrechargeStart + PCC_MIN_TIME_MS) {    // Precharge too fast - something's wrong!
+    if (prechargeProgress >= PCC_TARGET_PERCENT) {
+        if ((now - lastTimeBelowThreshold) > hyst) {
+            if (now <
+                timePrechargeStart + PCC_MIN_TIME_MS) { // Precharge too fast -
+                                                        // something's wrong!
                 state = STATE_ERROR;
                 errorCode |= ERR_PRECHARGE_TOO_FAST;
             }
@@ -211,13 +223,13 @@ void precharge(){
                 Serial.print("%   ");
                 Serial.print(tsVoltage, 1);
                 Serial.print("V\n");
-        }
-   
-
+            }
         }
 
     } else {
-        if (now > timePrechargeStart + PCC_MAX_TIME_MS) {       // Precharge too slow - something's wrong!
+        if (now >
+            timePrechargeStart +
+                PCC_MAX_TIME_MS) { // Precharge too slow - something's wrong!
             Serial.print(" * Precharge time: ");
             Serial.print(now - timePrechargeStart);
             Serial.print("\n");
@@ -229,8 +241,9 @@ void precharge(){
     }
 }
 
-// ONLINE STATE: Close AIR+ to connect ACC to TS, Open Precharge relay, indicate status
-void running(){
+// ONLINE STATE: Close AIR+ to connect ACC to TS, Open Precharge relay, indicate
+// status
+void running() {
     if (lastState != STATE_ONLINE) {
         lastState = STATE_ONLINE;
         Serial.println(" === ONLINE");
@@ -242,41 +255,44 @@ void running(){
 }
 
 // ERROR STATE: Indicate error, open AIRs and precharge relay
-void errorState(){
+void errorState() {
     digitalWrite(SHUTDOWN_CTRL_PIN, LOW);
 
-    if (lastState != STATE_ERROR){
+    if (lastState != STATE_ERROR) {
         lastState = STATE_ERROR;
         Serial.println(" === ERROR");
 
         // Display errors: Serial and Status LEDs
-        if (errorCode == ERR_NONE){
-        Serial.println("   *Error state, but no error code logged...");
+        if (errorCode == ERR_NONE) {
+            Serial.println("   *Error state, but no error code logged...");
         }
         if (errorCode & ERR_PRECHARGE_TOO_FAST) {
-        Serial.println("   *Precharge too fast. Suspect wiring fault / chatter in shutdown circuit.");
+            Serial.println("   *Precharge too fast. Suspect wiring fault / "
+                           "chatter in shutdown circuit.");
         }
         if (errorCode & ERR_PRECHARGE_TOO_SLOW) {
-        Serial.println("   *Precharge too slow. Potential causes:\n   - Wiring fault\n   - Discharge is stuck-on\n   - Target precharge percent is too high");
+            Serial.println("   *Precharge too slow. Potential causes:\n   - "
+                           "Wiring fault\n   - Discharge is stuck-on\n   - "
+                           "Target precharge percent is too high");
         }
         if (errorCode & ERR_STATE_UNDEFINED) {
-        Serial.println("   *State not defined in The State Machine.");
+            Serial.println("   *State not defined in The State Machine.");
         }
     }
 }
 
-float getTSVoltage(){
+float getTSVoltage() {
     // Get the tractive system voltage
     return tsVoltage;
 }
 
-float getAccumulatorVoltage(){
+float getAccumulatorVoltage() {
     // Get the accumulator voltage
     return accVoltage;
 }
 
 // Return current precharge state
-PrechargeState getPrechargeState(){
+PrechargeState getPrechargeState() {
     PrechargeState currentPrechargeState;
 
     taskENTER_CRITICAL(); // Ensure atomic access to state
@@ -287,7 +303,7 @@ PrechargeState getPrechargeState(){
 }
 
 // Obtain current error information
-int getPrechargeError(){
+int getPrechargeError() {
     int currentPrechargeError;
 
     taskENTER_CRITICAL(); // Ensure atomic access to error code
diff --git a/fsae-pcc/src/precharge.h b/fsae-pcc/src/precharge.h
index c78714d..646c67f 100644
--- a/fsae-pcc/src/precharge.h
+++ b/fsae-pcc/src/precharge.h
@@ -7,13 +7,14 @@
 #define TS_VOLTAGE_PIN 15
 #define PCC_RATIO .9
 
-
-#define PCC_MIN_TIME_MS 8200U // [ms] Minimum time to wait for precharge to complete
-#define PCC_MAX_TIME_MS 9200U // [ms] Maximum time to wait for precharge to complete
+#define PCC_MIN_TIME_MS                                                        \
+    8200U // [ms] Minimum time to wait for precharge to complete
+#define PCC_MAX_TIME_MS                                                        \
+    9200U // [ms] Maximum time to wait for precharge to complete
 #define PCC_TARGET_PERCENT 90U // Target precharge percent
 #define PCC_SETTLING_TIME 200U // [ms] Time to wait for precharge to settle
 #define PCC_MIN_ACC_VOLTAGE 1U // [V] Minimum voltage for shutdown circuit
-#define PCC_WAIT_TIME 200U // [ms] Time to wait for stable voltage
+#define PCC_WAIT_TIME 200U     // [ms] Time to wait for stable voltage
 
 enum PrechargeState {
     STATE_STANDBY,
@@ -28,7 +29,6 @@ enum {
     ERR_PRECHARGE_TOO_FAST = 0b00000001,
     ERR_PRECHARGE_TOO_SLOW = 0b00000010,
 
-
     ERR_STATE_UNDEFINED = 0b10000000,
 };
 
diff --git a/fsae-pcc/src/utils.h b/fsae-pcc/src/utils.h
index 5e18a2e..6a438a7 100644
--- a/fsae-pcc/src/utils.h
+++ b/fsae-pcc/src/utils.h
@@ -17,7 +17,8 @@
 
 // Lowpass filter
 #define TIME_STEP 0.001F // 1ms time step
-#define COMPUTE_ALPHA(CUTOFF_HZ) (1.0F / (1.0F + (1.0F / (2.0F * M_PI * CUTOFF_HZ)) / TIME_STEP))
+#define COMPUTE_ALPHA(CUTOFF_HZ)                                               \
+    (1.0F / (1.0F + (1.0F / (2.0F * M_PI * CUTOFF_HZ)) / TIME_STEP))
 #define LOWPASS_FILTER(NEW, OLD, ALPHA) OLD = ALPHA * NEW + (1.0F - ALPHA) * OLD
 
 #define Rs 12000
diff --git a/fsae-vehicle-fw/file_list.txt b/fsae-vehicle-fw/file_list.txt
new file mode 100644
index 0000000..e69de29
diff --git a/fsae-vehicle-fw/src/main.cpp b/fsae-vehicle-fw/src/main.cpp
index bd4137b..94f86b4 100644
--- a/fsae-vehicle-fw/src/main.cpp
+++ b/fsae-vehicle-fw/src/main.cpp
@@ -6,16 +6,19 @@
 #include "peripherals/adc.h"
 #include "peripherals/can.h"
 
+#include "peripherals/gpio.h"
 #include "vehicle/apps.h"
 #include "vehicle/bse.h"
 #include "vehicle/faults.h"
+#include "vehicle/ifl100-36.h"
 #include "vehicle/motor.h"
+#include "vehicle/pcc_receive.h"
+#include "vehicle/rtm_button.h"
 #include "vehicle/telemetry.h"
-#include "vehicle/ifl100-36.h"
 
+#include "utils/utils.h"
 #include <iostream>
 #include <unistd.h>
-#include "utils/utils.h"
 
 #define TORQUE_STEP 1
 #define TORQUE_MAX_NM 20 // Maximum torque demand in Nm
@@ -33,11 +36,18 @@ void setup() { // runs once on bootup
     Telemetry_Init();
     Motor_Init();
     MCU_Init();
-
-    xTaskCreate(threadADC, "threadADC", THREAD_ADC_STACK_SIZE, NULL, THREAD_ADC_PRIORITY, NULL);
-    xTaskCreate(threadMotor, "threadMotor", THREAD_MOTOR_STACK_SIZE, NULL, THREAD_MOTOR_PRIORITY, NULL);
-    xTaskCreate(threadTelemetry, "threadTelemetryCAN", THREAD_CAN_TELEMETRY_STACK_SIZE, NULL, THREAD_CAN_TELEMETRY_PRIORITY, NULL);
-    xTaskCreate(threadMain, "threadMain", THREAD_MAIN_STACK_SIZE, NULL, THREAD_MAIN_PRIORITY, NULL);
+    GPIO_Init();
+    PCC_Init();
+
+    xTaskCreate(threadADC, "threadADC", THREAD_ADC_STACK_SIZE, NULL,
+                THREAD_ADC_PRIORITY, NULL);
+    xTaskCreate(threadMotor, "threadMotor", THREAD_MOTOR_STACK_SIZE, NULL,
+                THREAD_MOTOR_PRIORITY, NULL);
+    xTaskCreate(threadTelemetry, "threadTelemetryCAN",
+                THREAD_CAN_TELEMETRY_STACK_SIZE, NULL,
+                THREAD_CAN_TELEMETRY_PRIORITY, NULL);
+    xTaskCreate(threadMain, "threadMain", THREAD_MAIN_STACK_SIZE, NULL,
+                THREAD_MAIN_PRIORITY, NULL);
     vTaskStartScheduler();
 }
 
@@ -45,125 +55,161 @@ void threadMain(void *pvParameters) {
     Serial.begin(9600);
     xLastWakeTime = xTaskGetTickCount(); // Initialize the last wake time
 
-    # if DEBUG_FLAG
+#if HIMAC_FLAG
     float torqueDemand = 0;
-    bool enablePrecharge = false;
+    bool enableStandby = false;
+    bool enablePrecharge = true;
     bool enablePower = false;
     bool enableRun = false;
+
     bool enableRegen = false;
-    # endif
+
+#endif
     while (true) {
+        digitalWrite(13, 1);
         /*
-            * Read user input from Serial to control torque demand.
-            * 'w' or 'W' to increase torque demand,
-            * 's' or 'S' to decrease torque demand,
-            * 'p' or 'P' to enter precharging state from standby,
-            * 'o' or 'O' to enter run state,
-            * ' ' (space) to stop all torque.
-            * The torque demand is limited between 0 and TORQUE_MAX_NM.
-            *
-            * Telemetry: battery current, phase current, motor speed, temperature(s)
-        */
-
-       # if DEBUG_FLAG
+         * Read user input from Serial to control torque demand.
+         * 'w' or 'W' to increase torque demand,
+         * 's' or 'S' to decrease torque demand,
+         *
+         * 'p' or 'P' to enter precharging state from standby,
+         * 'l' or 'L' to enter standby from precharging
+         *
+         * 'o' or 'O' to enter run state,
+         * 'k' or 'K' to go back to idle from run
+         *
+         * ' ' (space) to stop all torque. (reset w/s to 0)
+         * R to toggle regen
+         *
+         * The torque demand is limited between 0 and TORQUE_MAX_NM.
+         *
+         * Telemetry: battery current, phase current, motor speed,
+         * temperature(s)
+         */
+
+#if HIMAC_FLAG
         if (Serial.available()) {
             char input = Serial.read();
 
             switch (input) {
-                case 'w': // Increase torque demand
-                case 'W':
-                {
-                    if (torqueDemand < TORQUE_MAX_NM) {
-                        torqueDemand += TORQUE_STEP; // Increment torque demand
-                    }
-                    break;
-                }
-                case 's': // Decrease torque demand
-                case 'S':
-                {
-                    if( torqueDemand > 0) {
-                        torqueDemand -= TORQUE_STEP; // Decrement torque demand
-                    }
-                    break;
-                }
-                case 'p': // Precharge state
-                case 'P':
-                {
-                    enablePrecharge = true; // Set flag to enable precharging
-                    enablePower = false; // Disable run state
-                    enableRun = false; // Disable run state
-                    torqueDemand = 0; // Reset torque demand
-                    // Serial.println("Entering precharge state...");
+            case 'w': // Increase torque demand
+            case 'W': {
+                if (torqueDemand < TORQUE_MAX_NM) {
+                    torqueDemand += TORQUE_STEP; // Increment torque demand
                 }
-                    break;
-                case 'o': // Power Ready state
-                case 'O':
-                    {
-                        enablePrecharge = false; // Disable precharging
-                        enablePower = true; // Set flag to enable power ready state
-                        enableRun = false; // Set flag to enable run state
-                    }
                 break;
-
-                case 'i':
-                case 'I': // Run state
-                    {
-                        enablePrecharge = false; // Disable precharging
-                        enablePower = false; // Set flag to enable power ready state
-                        enableRun = true; // Set flag to enable run state
-                    }
-                    break;
-
-                case ' ': // Stop all torque
-                    torqueDemand = 0;
-                    enableRun = false; // Disable run state
-                    enablePrecharge = false; // Disable precharging
-                    break;
-                case 'f': // Fault state
-                case 'F': {
-                    Serial.println("Entering fault state");
-                    enableRun = false; // Disable run state
-                    enablePrecharge = false; // Disable precharging
-                    torqueDemand = 0; // Reset torque demand
-                    Motor_SetFaultState(); // Set motor to fault state
-                    break;
+            }
+            case 's': // Decrease torque demand
+            case 'S': {
+                if (torqueDemand > 0) {
+                    torqueDemand -= TORQUE_STEP; // Decrement torque demand
                 }
-                case 'r':
-                case 'R':
-                    enableRegen = !enableRegen;
-                default:
-                    break;
+                break;
+            }
+            case 'l':
+            case 'L': { // Standby state
+                enableStandby = true;
+                enablePrecharge = false; // Disable Precharge
+                enablePower = false;     // Disable run state
+                enableRun = false;       // Disable run state
+                break;
+            }
+            case 'p': // Precharge state
+            case 'P': {
+                enableStandby = false;
+                enablePrecharge = true; // Set flag to enable precharging
+                enablePower = false;    // Disable run state
+                enableRun = false;      // Disable run state
+                torqueDemand = 0;       // Reset torque demand
+                // Serial.println("Entering precharge state...");
+                break;
+            }
+            case 'o': // IDLE: Power Ready state
+            case 'O': {
+                enableStandby = false;
+                enablePrecharge = false; // Disable precharging
+                enablePower = true;      // Set flag to enable power ready state
+                enableRun = false;       // Set flag to enable run state
+                break;
+            }
+            case 'i':
+            case 'I': { // Run state
+                enableStandby = false;
+                enablePrecharge = false; // Disable precharging
+                enablePower = false;     // Set flag to enable power ready state
+                enableRun = true;        // Set flag to enable run state
+                break;
+            }
+            case ' ': { // Stop all torque
+                torqueDemand = 0;
+                // enableRun = false;       // Disable run state
+                // enablePrecharge = false; // Disable precharging
+                // enablePower = false;
+                // enableRun = false;
+                break;
+            }
+            case 'f': // Fault state
+            case 'F': {
+                // Serial.println("Entering fault state");
+                enableRun = false;       // Disable run state
+                enablePrecharge = false; // Disable precharging
+                enablePower = false;
+                torqueDemand = 0;      // Reset torque demand
+                Motor_SetFaultState(); // Set motor to fault state
+                break;
+            }
+            case 'r':
+            case 'R': {
+                enableRegen = !enableRegen;
+                break;
+            }
+            default:
+                break;
             }
         }
 
-
         // Serial.print("State: ");
         // Serial.print(MCU_GetMCU1Data().mcuMainState);
         // Serial.print(" | ");
         // Serial.print("Internal State: ");
         // Serial.print(Motor_GetState());
-        //Serial.print("      \n");
-
+        // Serial.print("      \n");
 
         // Serial.print("Torque - ");
         // Serial.print(torqueDemand);
         // Serial.print("      \n");
 
-        // Telemetry: Read battery current, phase current, motor speed, temperature(s)
+        // Telemetry: Read battery current, phase current, motor speed,
+        // temperature(s)
+        Serial.print("PP:");
+        Serial.print(PCC_GetData()->prechargeProgress);
+        Serial.print(" | ");
         Serial.print("C State: ");
         Serial.print(MCU_GetMCU1Data()->mcuMainState);
         Serial.print(" | ");
+        // Serial.print("APPS: ");
+        // Serial.print(APPS_GetAPPSReading1());
+        // Serial.print(" | ");
+        // Serial.print(APPS_GetAPPSReading2());
+        // Serial.print(" | ");
         Serial.print("T State: ");
         Serial.print(Motor_GetState());
         Serial.print(" | ");
 
         Serial.print("Torque: ");
         Serial.print(torqueDemand);
+        // Serial.print(" | ");
+        // Serial.print("M: ");
+        // Serial.print(MCU_GetMCU1Data()->motorTorque);
         Serial.print(" | ");
         Serial.print("RPM: ");
         Serial.print(MCU_GetMCU1Data()->motorSpeed);
 
-        // Telemetry: Read battery current, phase current, motor speed, temperature(s)
+        // Serial.print(" | ");
+        //  Serial.print("maxtorq ");
+        //  Serial.print(MCU_GetMCU1Data()->maxMotorTorque);
+        //  Telemetry: Read battery current, phase current, motor speed,
+        //  temperature(s)
         Serial.print(" | ");
         Serial.print("B Volt: ");
         Serial.print(MCU_GetMCU3Data()->mcuVoltage);
@@ -183,10 +229,10 @@ void threadMain(void *pvParameters) {
         Serial.print("Mtr Temp: ");
         Serial.print(MCU_GetMCU2Data()->motorTemp);
 
-        Serial.print(" | ");
-        Serial.print("Regen: ");
-        Serial.print(enableRegen);
-
+        // Serial.print(" | ");
+        // Serial.print("Regen: ");
+        // Serial.print(enableRegen);
+        Serial.print("\r");
 
         // Serial.print("Battery Current: ");
         // Serial.print(MCU_GetMCU3Data().mcuCurrent);
@@ -205,26 +251,108 @@ void threadMain(void *pvParameters) {
         // Serial.print("      \n");
 
         // print all errors if they are true in one line
-        // Serial.print("  |  ");
-        // if (MCU_GetMCU2Data().dcMainWireOverVoltFault) Serial.print("DC Over Volt Fault, ");
-        // if (MCU_GetMCU2Data().motorPhaseCurrFault) Serial.print("Motor Phase Curr Fault, ");
-        // if (MCU_GetMCU2Data().mcuOverHotFault) Serial.print("MCU Over Hot Fault, ");
-        // if (MCU_GetMCU2Data().resolverFault) Serial.print("Resolver Fault, ");
-        // if (MCU_GetMCU2Data().phaseCurrSensorFault) Serial.print("Phase Curr Sensor Fault, ");
-        // if (MCU_GetMCU2Data().motorOverSpdFault) Serial.print("Motor Over Spd Fault, ");
-        // if (MCU_GetMCU2Data().drvMotorOverHotFault) Serial.print("Driver Motor Over Hot Fault, ");
-        // if (MCU_GetMCU2Data().dcMainWireOverCurrFault) Serial.print("DC Main Wire Over Curr Fault, ");
-        // if (MCU_GetMCU2Data().drvMotorOverCoolFault) Serial.print("Driver Motor Over Cool Fault, ");
-        // if (MCU_GetMCU2Data().dcLowVoltWarning) Serial.print("DC Low Volt Warning, ");
-        // if (MCU_GetMCU2Data().mcu12VLowVoltWarning) Serial.print("MCU 12V Low Volt Warning, ");
-        // if (MCU_GetMCU2Data().motorStallFault) Serial.print("Motor Stall Fault, ");
-        // if (MCU_GetMCU2Data().motorOpenPhaseFault) Serial.print("Motor Open Phase Fault, ");
-
-        Motor_UpdateMotor(torqueDemand, enablePrecharge, enablePower, enableRun, enableRegen); // Update motor with the current torque demand
-        # endif
+        Serial.print("  |  ");
+
+        if (MCU_GetMCU2Data()->dcMainWireOverVoltFault) {
+            Faults_SetFault(FAULT_DC_OVER_VOLT);
+            Serial.println("DC Over Volt Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_DC_OVER_VOLT);
+        }
+
+        if (MCU_GetMCU2Data()->motorPhaseCurrFault) {
+            Faults_SetFault(FAULT_MOTOR_PHASE_CURR);
+            Serial.println("Motor Phase Curr Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_MOTOR_PHASE_CURR);
+        }
+
+        if (MCU_GetMCU2Data()->mcuOverHotFault) {
+            Faults_SetFault(FAULT_OVER_HOT);
+            Serial.println("MCU Over Hot Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_OVER_HOT);
+        }
+
+        if (MCU_GetMCU2Data()->resolverFault) {
+            Faults_SetFault(FAULT_RESOLVER);
+            Serial.println("Resolver Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_RESOLVER);
+        }
+
+        if (MCU_GetMCU2Data()->phaseCurrSensorFault) {
+            Faults_SetFault(FAULT_PHASE_CURR_SENSOR);
+            Serial.println("Phase Curr Sensor Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_PHASE_CURR_SENSOR);
+        }
+
+        if (MCU_GetMCU2Data()->motorOverSpdFault) {
+            Faults_SetFault(FAULT_MOTOR_OVER_SPEED);
+            Serial.println("Motor Over Spd Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_MOTOR_OVER_SPEED);
+        }
+
+        if (MCU_GetMCU2Data()->drvMotorOverHotFault) {
+            Faults_SetFault(FAULT_DRV_OVER_HOT);
+            Serial.println("Driver Motor Over Hot Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_DRV_OVER_HOT);
+        }
+
+        if (MCU_GetMCU2Data()->dcMainWireOverCurrFault) {
+            Faults_SetFault(FAULT_DC_OVER_CURR);
+            Serial.println("DC Main Wire Over Curr Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_DC_OVER_CURR);
+        }
+
+        if (MCU_GetMCU2Data()->drvMotorOverCoolFault) {
+            Faults_SetFault(FAULT_DRV_OVER_COOL);
+            Serial.println("Driver Motor Over Cool Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_DRV_OVER_COOL);
+        }
+
+        if (MCU_GetMCU2Data()->dcLowVoltWarning) {
+            Faults_SetFault(FAULT_DC_LOW_VOLT_WARN);
+            Serial.println("DC Low Volt Warning, ");
+        } else {
+            Faults_ClearFault(FAULT_DC_LOW_VOLT_WARN);
+        }
+
+        if (MCU_GetMCU2Data()->mcu12VLowVoltWarning) {
+            Faults_SetFault(FAULT_12V_LOW_VOLT_WARN);
+            Serial.println("MCU 12V Low Volt Warning, ");
+        } else {
+            Faults_ClearFault(FAULT_12V_LOW_VOLT_WARN);
+        }
+
+        if (MCU_GetMCU2Data()->motorStallFault) {
+            Faults_SetFault(FAULT_MOTOR_STALL);
+            Serial.println("Motor Stall Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_MOTOR_STALL);
+        }
+
+        if (MCU_GetMCU2Data()->motorOpenPhaseFault) {
+            Faults_SetFault(FAULT_MOTOR_OPEN_PHASE);
+            Serial.println("Motor Open Phase Fault, ");
+        } else {
+            Faults_ClearFault(FAULT_MOTOR_OPEN_PHASE);
+        }
+
+
+        Motor_UpdateMotor(
+            torqueDemand, enablePrecharge, enablePower, enableRun, enableRegen,
+            enableStandby); // Update motor with the current torque demand
+
+#endif
+        // Serial.print(RTMButton_GetState());
         vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10)); // Delay for 100ms
     }
 }
 
 void loop() {}
-
diff --git a/fsae-vehicle-fw/src/peripherals/adc.cpp b/fsae-vehicle-fw/src/peripherals/adc.cpp
index c43b595..bb8b49f 100644
--- a/fsae-vehicle-fw/src/peripherals/adc.cpp
+++ b/fsae-vehicle-fw/src/peripherals/adc.cpp
@@ -1,27 +1,32 @@
 // Anteater Electric Racing, 2025
 
-#include <ADC.h>
-#include <stdint.h>
 #include "adc.h"
 #include "./vehicle/telemetry.h"
-#include <chrono>
-#include <arduino_freertos.h>
 #include "utils/utils.h"
-#include "vehicle/bse.h"
 #include "vehicle/apps.h"
+#include "vehicle/bse.h"
 #include "vehicle/faults.h"
-#include "vehicle/telemetry.h"
 #include "vehicle/motor.h"
+#include "vehicle/telemetry.h"
+#include "vehicle/thermal.h"
+#include <ADC.h>
+#include <arduino_freertos.h>
+#include <chrono>
+#include <stdint.h>
 
-enum SensorIndexesADC0 { // TODO: Update with real values
-    APPS_1_INDEX,
-    APPS_2_INDEX,
-    BSE_1_INDEX,
-    BSE_2_INDEX,
+enum SensorIndexesADC0 {    // TODO: Update with real values
+    THERMISTOR_1_INDEX = 0, // A0
+    APPS_1_INDEX = 4,
+    APPS_2_INDEX = 5, // A5
+    BSE_1_INDEX = 3,
+    BSE_2_INDEX = 2,
     SUSP_TRAV_LINPOT1,
     SUSP_TRAV_LINPOT2,
     SUSP_TRAV_LINPOT3,
-    SUSP_TRAV_LINPOT4
+    SUSP_TRAV_LINPOT4,
+    THERMISTOR_2_INDEX = 10, // A1
+    THERMISTOR_3_INDEX = 9,  // A2
+    THERMISTOR_4_INDEX = 8   // A3
 };
 
 enum SensorIndexesADC1 { // TODO: Update with real values
@@ -35,10 +40,16 @@ enum SensorIndexesADC1 { // TODO: Update with real values
     SUSP_TRAV_LINPOT42
 };
 
-uint16_t adc0Pins[SENSOR_PIN_AMT_ADC0] = {A0, A1, A2, A3, A4, A5, A6, A7}; // A4, A4, 18, 17, 17, 17, 17}; // real values: {21, 24, 25, 19, 18, 14, 15, 17};
+uint16_t adc0Pins[SENSOR_PIN_AMT_ADC0] = {
+    A0, A1, A2, A3,  A4, A5,
+    A6, A7, A9, A16, A17}; // A4, A4, 18, 17, 17, 17, 17}; // real values: {21,
+                           // 24, 25, 19, 18, 14, 15, 17};
 uint16_t adc0Reads[SENSOR_PIN_AMT_ADC0];
 
-uint16_t adc1Pins[SENSOR_PIN_AMT_ADC1] = {A7, A6, A5, A4, A3, A2, A1, A0}; // A4, A4, 18, 17, 17, 17, 17}; // real values: {21, 24, 25, 19, 18, 14, 15, 17};
+uint16_t adc1Pins[SENSOR_PIN_AMT_ADC1] = {
+    A17, A16, A15, A7, A6, A5,
+    A4,  A3,  A2,  A1, A0}; // A4, A4, 18, 17, 17, 17, 17}; // real values: {21,
+                            // 24, 25, 19, 18, 14, 15, 17};
 uint16_t adc1Reads[SENSOR_PIN_AMT_ADC1];
 
 static TickType_t lastWakeTime;
@@ -47,46 +58,68 @@ ADC *adc = new ADC();
 
 void ADC_Init() {
     // ADC 0
-    adc->adc0->setAveraging(ADC_AVERAGING); // set number of averages
+    adc->adc0->setAveraging(ADC_AVERAGING);   // set number of averages
     adc->adc0->setResolution(ADC_RESOLUTION); // set bits of resolution
-    adc->adc0->setConversionSpeed(ADC_CONVERSION_SPEED::LOW_SPEED); // change the conversion speed
-    adc->adc0->setSamplingSpeed(ADC_SAMPLING_SPEED::LOW_SPEED); // change the sampling speed
+    adc->adc0->setConversionSpeed(
+        ADC_CONVERSION_SPEED::LOW_SPEED); // change the conversion speed
+    adc->adc0->setSamplingSpeed(
+        ADC_SAMPLING_SPEED::LOW_SPEED); // change the sampling speed
 
     // ADC 1
-    adc->adc1->setAveraging(ADC_AVERAGING); // set number of averages
+    adc->adc1->setAveraging(ADC_AVERAGING);   // set number of averages
     adc->adc1->setResolution(ADC_RESOLUTION); // set bits of resolution
-    adc->adc1->setConversionSpeed(ADC_CONVERSION_SPEED::LOW_SPEED); // change the conversion speed
-    adc->adc1->setSamplingSpeed(ADC_SAMPLING_SPEED::LOW_SPEED); // change the sampling speed
+    adc->adc1->setConversionSpeed(
+        ADC_CONVERSION_SPEED::LOW_SPEED); // change the conversion speed
+    adc->adc1->setSamplingSpeed(
+        ADC_SAMPLING_SPEED::LOW_SPEED); // change the sampling speed
 
-    # if DEBUG_FLAG
-        Serial.println("Done initializing ADCs");
-    # endif
+#if DEBUG_FLAG
+    Serial.println("Done initializing ADCs");
+#endif
 }
 
-void threadADC( void *pvParameters ){
-    # if DEBUG_FLAG
-        Serial.print("Beginning adc thread");
-    # endif
+void threadADC(void *pvParameters) {
+#if DEBUG_FLAG
+    Serial.print("Beginning adc thread");
+#endif
 
     lastWakeTime = xTaskGetTickCount();
-    while(true){
+    while (true) {
         vTaskDelayUntil(&lastWakeTime, TICKTYPE_FREQUENCY);
-        for(uint16_t currentIndexADC0 = 0; currentIndexADC0 < SENSOR_PIN_AMT_ADC0; ++currentIndexADC0){
+        for (uint16_t currentIndexADC0 = 0;
+             currentIndexADC0 < SENSOR_PIN_AMT_ADC0; ++currentIndexADC0) {
             uint16_t currentPinADC0 = adc0Pins[currentIndexADC0];
-            uint16_t adcRead = adc->adc1->analogRead(currentPinADC0);
+            uint16_t adcRead = adc->adc0->analogRead(currentPinADC0);
             adc0Reads[currentIndexADC0] = adcRead;
         }
 
-        for(uint16_t currentIndexADC1 = 0; currentIndexADC1 < SENSOR_PIN_AMT_ADC1; ++currentIndexADC1){
+        for (uint16_t currentIndexADC1 = 0;
+             currentIndexADC1 < SENSOR_PIN_AMT_ADC1; ++currentIndexADC1) {
             uint16_t currentPinADC1 = adc1Pins[currentIndexADC1];
             uint16_t adcRead = adc->adc1->analogRead(currentPinADC1);
             adc1Reads[currentIndexADC1] = adcRead;
         }
 
+        // Serial.print("ADC0 Reads: ");
+        // for (int i = 0; i < SENSOR_PIN_AMT_ADC0; i++) {
+        //     Serial.print(adc0Reads[i]);
+        //     Serial.print(" ");
+        // }
+        // Serial.println();
+        // Serial.print("ADC1 Reads: ");
+        // for (int i = 0; i < SENSOR_PIN_AMT_ADC1; i++) {
+        //     Serial.print(adc1Reads[i]);
+        //     Serial.print(" ");
+        // }
+        // Serial.println();
         // Update each sensors data
         APPS_UpdateData(adc0Reads[APPS_1_INDEX], adc0Reads[APPS_2_INDEX]);
         BSE_UpdateData(adc0Reads[BSE_1_INDEX], adc0Reads[BSE_2_INDEX]);
 
+        thermal_Update(
+            adc0Reads[THERMISTOR_1_INDEX], adc0Reads[THERMISTOR_2_INDEX],
+            adc0Reads[THERMISTOR_3_INDEX], adc0Reads[THERMISTOR_4_INDEX]);
+
         // Handle any faults that were raised
         // Faults_HandleFaults();
         // Motor_UpdateMotor();
diff --git a/fsae-vehicle-fw/src/peripherals/adc.h b/fsae-vehicle-fw/src/peripherals/adc.h
index 1108ec9..9a2f8f6 100644
--- a/fsae-vehicle-fw/src/peripherals/adc.h
+++ b/fsae-vehicle-fw/src/peripherals/adc.h
@@ -2,8 +2,8 @@
 #pragma once
 
 #include <ADC.h>
-#define SENSOR_PIN_AMT_ADC0 8
-#define SENSOR_PIN_AMT_ADC1 8
+#define SENSOR_PIN_AMT_ADC0 11
+#define SENSOR_PIN_AMT_ADC1 11
 
 extern uint16_t adc0Pins[SENSOR_PIN_AMT_ADC0];
 extern uint16_t adc0Index;
@@ -16,4 +16,4 @@ extern uint16_t adc1Reads[SENSOR_PIN_AMT_ADC1];
 extern ADC *adc;
 
 void ADC_Init();
-void threadADC( void *pvParameters );
+void threadADC(void *pvParameters);
diff --git a/fsae-vehicle-fw/src/peripherals/can.cpp b/fsae-vehicle-fw/src/peripherals/can.cpp
index 473deea..b5d77b5 100644
--- a/fsae-vehicle-fw/src/peripherals/can.cpp
+++ b/fsae-vehicle-fw/src/peripherals/can.cpp
@@ -3,15 +3,15 @@
 #define THREAD_CAN_STACK_SIZE 128
 #define THREAD_CAN_PRIORITY 1
 
-#include <stdint.h>
 #include <isotp.h>
 #include <FlexCAN_T4.h>
 #include <arduino_freertos.h>
+#include <stdint.h>
 
 #include "peripherals/can.h"
 #include "utils/utils.h"
-#include "vehicle/motor.h"
 #include "vehicle/ifl100-36.h"
+#include "vehicle/motor.h"
 
 #define CAN_INSTANCE CAN1
 #define CAN_BAUD_RATE 500000
@@ -23,6 +23,24 @@ isotp<RX_BANKS_16, 512> tp;
 CAN_message_t motorMsg;
 CAN_message_t rx_msg;
 
+// CAN loss detection
+// static uint32_t lastCAN2MsgTime = 0;
+// static uint32_t lastCAN3MsgTime = 0;
+static bool can2Healthy = false;
+static bool can3Healthy = false;
+
+#define CAN_TIMEOUT_MS 100
+
+//
+/**
+ * CANBus Setup: KZ
+ *
+ * CAN2 (CAN2 on diagram) --> ORION, CCM, RASPI, PCC
+ * CAN3 (CAN1 on diagram) --> CCM, OMNI, RASPI
+ *
+ * TODO: CAN recieve and send for each bus
+ *
+ */
 void CAN_Init() {
     // Initialize CAN bus
     can2.begin();
@@ -43,28 +61,57 @@ void CAN_Init() {
     tp.setWriteBus(&can3); // Set the bus to write to can3
 }
 
-void CAN_Send(uint32_t id, uint64_t msg)
-{
+// TODO @ksthakkar: make can_send general function to select both CANbuses or
+// make seperate CAN_send functions can2
+void CAN_Send(uint32_t id, uint64_t msg) {
     motorMsg.id = id;
+    // msg.id = 0x100;
     memcpy(motorMsg.buf, &msg, sizeof(msg));
 
-    can2.write(motorMsg);
+    can3.write(motorMsg);
+
+    // Serial.println("| Sending CAN... ");
+    // Serial.print("");
 }
 
-void CAN_Receive(uint32_t* rx_id, uint64_t* rx_data) {
-    if (can2.read(rx_msg)) {
+void CAN_Receive(uint32_t *rx_id, uint64_t *rx_data) {
+    if (can3.read(rx_msg)) {
         *rx_id = rx_msg.id;
         memcpy(rx_data, rx_msg.buf, sizeof(*rx_data));
+
+        // Serial.println("--> CAN Message recieved");
+        // lastCAN2MsgTime = millis();
+        // can2Healthy = true;
     } else { // No message received, assign default values
         *rx_id = 0;
         *rx_data = 0;
     }
 }
 
-void CAN_ISOTP_Send(uint32_t id, uint8_t* msg, uint16_t size) {
+void CAN_CheckHealth() {
+    // uint32_t now = millis();
+    // if (now - lastCAN2MsgTime > CAN_TIMEOUT_MS) {
+    //     can2Healthy = false;
+    // }
+
+    // if (now - lastCAN3MsgTime > CAN_TIMEOUT_MS) {
+    //     can3Healthy = false;
+    // }
+}
+
+void CAN_ISOTP_Send(uint32_t id, uint8_t *msg, uint16_t size) {
     ISOTP_data config;
     config.id = id;
     config.flags.extended = 0; // Standard frame
-    config.separation_time = 1; // Time between back-to-back frames in milliseconds
+    config.separation_time =
+        1; // Time between back-to-back frames in milliseconds
     tp.write(config, msg, size);
 }
+
+bool CAN_IsBusHealthy(uint8_t bus) {
+    if (bus == 2)
+        return can2Healthy;
+    if (bus == 3)
+        return can3Healthy;
+    return false;
+}
diff --git a/fsae-vehicle-fw/src/peripherals/can.h b/fsae-vehicle-fw/src/peripherals/can.h
index cf71aac..203a94e 100644
--- a/fsae-vehicle-fw/src/peripherals/can.h
+++ b/fsae-vehicle-fw/src/peripherals/can.h
@@ -4,6 +4,9 @@
 
 void CAN_Init();
 void CAN_Send(uint32_t id, uint64_t msg);
-void CAN_Receive(uint32_t* rx_id, uint64_t* rx_data);
+void CAN_Receive(uint32_t *rx_id, uint64_t *rx_data);
 
-void CAN_ISOTP_Send(uint32_t id, uint8_t* msg, uint16_t size);
+void CAN_ISOTP_Send(uint32_t id, uint8_t *msg, uint16_t size);
+
+bool CAN_IsBusHealthy(uint8_t bus);
+void CAN_CheckHealth();
diff --git a/fsae-vehicle-fw/src/peripherals/gpio.cpp b/fsae-vehicle-fw/src/peripherals/gpio.cpp
index 8f12bf1..275e125 100644
--- a/fsae-vehicle-fw/src/peripherals/gpio.cpp
+++ b/fsae-vehicle-fw/src/peripherals/gpio.cpp
@@ -1,14 +1,24 @@
 // Anteater Electric Racing, 2025
 
-#include <Arduino.h>
 #include "peripherals/gpio.h"
+#include <Arduino.h>
 
 // rtm button: 4
 // wheel speed 1: 2, wheel speed 2: 3
 // using can1 and can2
 
 void GPIO_Init() {
-    pinMode(2, INPUT_PULLUP); // wheel speed 1
-    pinMode(3, INPUT_PULLUP); // wheel speed 2
+    pinMode(2, INPUT_PULLUP);   // wheel speed 1
+    pinMode(3, INPUT_PULLUP);   // wheel speed 2
     pinMode(4, INPUT_PULLDOWN); // rtm button
 }
+
+// Read the current logical level of a GPIO pin.
+// Uses digitalReadFast when available on the platform for better performance.
+int GPIO_Read(int pin) {
+#ifdef digitalReadFast
+    return digitalReadFast(pin);
+#else
+    return digitalRead(pin);
+#endif
+}
diff --git a/fsae-vehicle-fw/src/peripherals/gpio.h b/fsae-vehicle-fw/src/peripherals/gpio.h
index d7b381a..75f0d63 100644
--- a/fsae-vehicle-fw/src/peripherals/gpio.h
+++ b/fsae-vehicle-fw/src/peripherals/gpio.h
@@ -2,4 +2,10 @@
 
 #pragma once
 
+#include <stdint.h>
+
 void GPIO_Init();
+
+// Read the current logical level of a GPIO pin.
+// Returns 0 for LOW, non-zero for HIGH.
+int GPIO_Read(int pin);
diff --git a/fsae-vehicle-fw/src/peripherals/peripherals.cpp b/fsae-vehicle-fw/src/peripherals/peripherals.cpp
index 83e168b..bd9bfeb 100644
--- a/fsae-vehicle-fw/src/peripherals/peripherals.cpp
+++ b/fsae-vehicle-fw/src/peripherals/peripherals.cpp
@@ -1,8 +1,8 @@
 // Anteater Electric Racing, 2025
+#include "peripherals/peripherals.h"
 #include "peripherals/adc.h"
 #include "peripherals/can.h"
 #include "peripherals/gpio.h"
-#include "peripherals/peripherals.h"
 
 void Peripherals_Init() {
     ADC_Init();
diff --git a/fsae-vehicle-fw/src/utils/utils.h b/fsae-vehicle-fw/src/utils/utils.h
index 5233e7e..0096aae 100644
--- a/fsae-vehicle-fw/src/utils/utils.h
+++ b/fsae-vehicle-fw/src/utils/utils.h
@@ -3,6 +3,7 @@
 #pragma once
 
 #define DEBUG_FLAG 0
+#define HIMAC_FLAG 0
 
 #define THREAD_MAIN_STACK_SIZE 128
 #define THREAD_MAIN_PRIORITY 1
@@ -25,8 +26,10 @@
 #define ADC_MAX_VALUE ((1 << ADC_RESOLUTION) - 1)
 #define TICKTYPE_FREQUENCY 1
 
-#define ADC_VOLTAGE_DIVIDER 2.0F
-#define ADC_VALUE_TO_VOLTAGE(x) ((x) * (LOGIC_LEVEL_V / ADC_MAX_VALUE)) * ADC_VOLTAGE_DIVIDER
+#define ADC_VOLTAGE_DIVIDER 1.515151F
+
+#define ADC_VALUE_TO_VOLTAGE(x)                                                \
+    ((x) * (LOGIC_LEVEL_V * ADC_VOLTAGE_DIVIDER / ADC_MAX_VALUE))
 
 #define APPS_FAULT_PERCENT_MIN .1
 #define APPS_FAULT_PERCENT_MAX .9
@@ -43,6 +46,30 @@
 #define APPS_5V_MIN (APPS2_VOLTAGE_LEVEL * APPS_RANGE_MIN_PERCENT)
 #define APPS_5V_MAX (APPS2_VOLTAGE_LEVEL * APPS_RANGE_MAX_PERCENT)
 
+/*     ANOOP TESTING FOR 20% HERE     */
+
+// APPS 0-20% -> 0-100% scaling
+
+// ADC values corresponding to physical 20% pedal)
+#define APPS1_20PCT_ADC 784.0F
+#define APPS2_20PCT_ADC 1150.0F
+
+// Measured resting ADC (change these with actual findings this is just safe
+// zone values)
+#define APPS1_REST_ADC 11.75F
+#define APPS2_REST_ADC 17.5F
+
+// Clamp helper
+#define CLAMP(x, lo, hi) ((x) < (lo) ? (lo) : ((x) > (hi) ? (hi) : (x)))
+#define CLAMP01(x) CLAMP((x), 0.0F, 1.0F)
+
+// Convert raw ADC -> commanded percent using only 0-20% physical range
+#define APPS_ADC_TO_CMD_PERCENT(adc, rest_adc, adc_20)                         \
+    CLAMP01(((float)(adc) - (float)(rest_adc)) /                               \
+            ((float)(adc_20) - (float)(rest_adc)))
+
+/*     END ANOOP TESTING FOR 20% HERE     */
+
 #define APPS_3V3_FAULT_MIN (APPS1_VOLTAGE_LEVEL * APPS_FAULT_PERCENT_MIN)
 #define APPS_3V3_FAULT_MAX (APPS1_VOLTAGE_LEVEL * APPS_FAULT_PERCENT_MAX)
 
@@ -53,11 +80,14 @@
 
 #define APPS_IMPLAUSABILITY_THRESHOLD 0.1            // 10%
 #define APPS_BSE_PLAUSABILITY_TROTTLE_THRESHOLD 0.25 // 25%
-#define APPS_BSE_PLAUSABILITY_BRAKE_THRESHOLD 1.5 // TODO: change back to PSI200    // PSI
+#define APPS_BSE_PLAUSABILITY_BRAKE_THRESHOLD                                  \
+    1.5 // TODO: change back to PSI200    // PSI
 #define APPS_BSE_PLAUSIBILITY_RESET_THRESHOLD 0.05 // 5%
 
 #define BSE_VOLTAGE_DIVIDER 2.0F // TODO: Update with real value
-#define BSE_ADC_VALUE_TO_VOLTAGE(x) (x * (LOGIC_LEVEL_V / ADC_MAX_VALUE)) * BSE_VOLTAGE_DIVIDER // ADC value to voltage conversion
+#define BSE_ADC_VALUE_TO_VOLTAGE(x)                                            \
+    (x * (LOGIC_LEVEL_V / ADC_MAX_VALUE)) *                                    \
+        BSE_VOLTAGE_DIVIDER // ADC value to voltage conversion
 
 #define BSE_VOLTAGE_TO_PSI(x) x // Voltage to PSI conversion
 
@@ -74,17 +104,15 @@
 #define BATTERY_MAX_CURRENT_A 1.0F
 #define BATTERY_MAX_REGEN_A 1.0F
 
-#define COMPUTE_ALPHA(CUTOFF_HZ) \
+#define COMPUTE_ALPHA(CUTOFF_HZ)                                               \
     (1.0F / (1.0F + (1.0F / (2.0F * M_PI * CUTOFF_HZ)) / TIME_STEP))
 
-#define LOWPASS_FILTER(NEW, OLD, ALPHA) \
-    OLD = ALPHA * NEW + (1.0F - ALPHA) * OLD
+#define LOWPASS_FILTER(NEW, OLD, ALPHA) OLD = ALPHA * NEW + (1.0F - ALPHA) * OLD
 
-#define LINEAR_MAP(x, in_min, in_max, out_min, out_max) \
+#define LINEAR_MAP(x, in_min, in_max, out_min, out_max)                        \
     ((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
 
-#define CHANGE_ENDIANESS_16(x) \
-    (((x & 0xFF00) >> 8) | ((x & 0x00FF) << 8))
+#define CHANGE_ENDIANESS_16(x) (((x & 0xFF00) >> 8) | ((x & 0x00FF) << 8))
 
 #define MOTOR_DIRECTION_STANDBY 0
 #define MOTOR_DIRECTION_FORWARD 1
@@ -92,4 +120,4 @@
 #define MOTOR_DIRECTION_ERROR 3
 
 #define MAX_REGEN_TORQUE -9.0F // TODO: test with higher value regen
-#define REGEN_BIAS 1 // Scale 0-1 of max regen torque
+#define REGEN_BIAS 1           // Scale 0-1 of max regen torque
diff --git a/fsae-vehicle-fw/src/vehicle/apps.cpp b/fsae-vehicle-fw/src/vehicle/apps.cpp
index fac83e8..4c8ed54 100644
--- a/fsae-vehicle-fw/src/vehicle/apps.cpp
+++ b/fsae-vehicle-fw/src/vehicle/apps.cpp
@@ -1,18 +1,18 @@
 // Anteater Electric Racing, 2025
 
-#include <cmath>
-#include "utils/utils.h"
 #include "apps.h"
+#include "utils/utils.h"
 #include "vehicle/faults.h"
 #include "vehicle/telemetry.h"
 #include <arduino_freertos.h>
+#include <cmath>
 
 typedef struct {
     float appsReading1_Percentage; // Percentage of pedal travel (0 to 1)
     float appsReading2_Percentage; // Percentage of pedal travel (0 to 1)
 
-    float appsReading1_Voltage;    // Voltage reading from the pedal (0 to 3.3V)
-    float appsReading2_Voltage;    // Voltage reading from the pedal (0 to 3.3V)
+    float appsReading1_Voltage; // Voltage reading from the pedal (0 to 3.3V)
+    float appsReading2_Voltage; // Voltage reading from the pedal (0 to 5V)
 
     float apps1RawReading;
     float apps2RawReading;
@@ -20,8 +20,8 @@ typedef struct {
 
 static APPSData appsData;
 static float appsAlpha;
-static TickType_t appsLatestHealthyStateTime = 0; // Set to 0 when fault not detected
-
+static TickType_t appsLatestHealthyStateTime =
+    0; // Set to 0 when fault not detected
 
 static void checkAndHandleAPPSFault();
 static void checkAndHandlePlausibilityFault();
@@ -39,28 +39,88 @@ void APPS_Init() {
     appsAlpha = COMPUTE_ALPHA(100.0F);
 }
 
-void APPS_UpdateData(uint32_t rawReading1, uint32_t rawReading2) {
-    // Filter incoming values
+void APPS_UpdateData(uint16_t rawReading1,
+                     uint16_t rawReading2) { // changed uint16 from 32
+    // Serial.print("Raw APPS1: ");
+    // Serial.println(rawReading1);
+    // Serial.print("Raw APPS2: ");
+    // Serial.println(rawReading2);
+
+    if (rawReading1 > APPS1_20PCT_ADC)
+        rawReading1 = APPS1_20PCT_ADC;
+    if (rawReading2 > APPS2_20PCT_ADC)
+        rawReading2 = APPS2_20PCT_ADC;
+
     LOWPASS_FILTER(rawReading1, appsData.apps1RawReading, appsAlpha);
     LOWPASS_FILTER(rawReading2, appsData.apps2RawReading, appsAlpha);
+    Serial.print("\n\n\n\n\n");
+    Serial.print("Raw APPS1: ");
+    Serial.println(appsData.apps1RawReading);
+    Serial.print("Raw APPS2: ");
+    Serial.println(appsData.apps2RawReading);
+
+    // after LOWPASS_FILTER
+    appsData.appsReading1_Percentage =
+        LINEAR_MAP(appsData.apps1RawReading, (float)APPS1_REST_ADC,
+                   (float)APPS1_20PCT_ADC, 0.0F, 1.0F);
+
+    appsData.appsReading2_Percentage =
+        LINEAR_MAP(appsData.apps2RawReading, (float)APPS2_REST_ADC,
+                   (float)APPS2_20PCT_ADC, 0.0F, 1.0F);
+
+    // clamp since LINEAR_MAP doesn't clamp
+    appsData.appsReading1_Percentage =
+        CLAMP01(appsData.appsReading1_Percentage);
+    appsData.appsReading2_Percentage =
+        CLAMP01(appsData.appsReading2_Percentage);
 
     // Convert ADC values to voltage
-    appsData.appsReading1_Voltage = ADC_VALUE_TO_VOLTAGE(appsData.apps1RawReading);
-    appsData.appsReading2_Voltage = ADC_VALUE_TO_VOLTAGE(appsData.apps2RawReading);
+    appsData.appsReading1_Voltage =
+        ADC_VALUE_TO_VOLTAGE(appsData.apps1RawReading);
+    appsData.appsReading2_Voltage =
+        ADC_VALUE_TO_VOLTAGE(appsData.apps2RawReading);
+
+    Serial.print("APPS1 RAW Voltage: ");
+    Serial.println(appsData.appsReading1_Voltage);
+    Serial.print("APPS2 RAW Voltage: ");
+    Serial.println(appsData.appsReading2_Voltage);
+
+    if (appsData.appsReading1_Voltage < APPS_3V3_MIN) {
+        appsData.appsReading1_Voltage = APPS_3V3_MIN;
+    } else if (appsData.appsReading1_Voltage > APPS_3V3_MAX) {
+        appsData.appsReading1_Voltage = APPS_3V3_MAX;
+    }
+
+    if (appsData.appsReading2_Voltage < APPS_5V_MIN) {
+        appsData.appsReading2_Voltage = APPS_5V_MIN;
+    } else if (appsData.appsReading2_Voltage > APPS_5V_MAX) {
+        appsData.appsReading2_Voltage = APPS_5V_MAX;
+    }
+    // Moved this upwards to before the clamping of percentage
 
     // Map voltage to percentage of throttle travel, limiting to 0-1 range
-    appsData.appsReading1_Percentage = LINEAR_MAP(appsData.appsReading1_Voltage, APPS_3V3_MIN, APPS_3V3_MAX, 0.0F, 1.0F);
-    appsData.appsReading2_Percentage = LINEAR_MAP(appsData.appsReading2_Voltage, APPS_5V_MIN, APPS_5V_MAX, 0.0F, 1.0F);
+    // appsData.appsReading1_Percentage =
+    //     LINEAR_MAP(appsData.apps1RawReading, 0.0F, (float)APPS1_20PCT_ADC,
+    //     0.0F, 1.0F);
 
-    if(appsData.appsReading1_Percentage < 0.0F) {
+    // appsData.appsReading2_Percentage =
+    //     LINEAR_MAP(appsData.apps2RawReading, 0.0F, (float)APPS2_20PCT_ADC,
+    //     0.0F, 1.0F);
+
+    // Serial.print("APPS1 raw Perc: ");
+    // Serial.println(appsData.appsReading1_Percentage);
+    // Serial.print("APPS2 raw Perc: ");
+    // Serial.println(appsData.appsReading2_Percentage);
+
+    if (appsData.appsReading1_Percentage < 0.0F) {
         appsData.appsReading1_Percentage = 0.0F;
-    } else if(appsData.appsReading1_Percentage > 1.0F) {
+    } else if (appsData.appsReading1_Percentage > 1.0F) {
         appsData.appsReading1_Percentage = 1.0F;
     }
 
-    if(appsData.appsReading2_Percentage < 0.0F) {
+    if (appsData.appsReading2_Percentage < 0.0F) {
         appsData.appsReading2_Percentage = 0.0F;
-    } else if(appsData.appsReading2_Percentage > 1.0F) {
+    } else if (appsData.appsReading2_Percentage > 1.0F) {
         appsData.appsReading2_Percentage = 1.0F;
     }
 
@@ -69,41 +129,43 @@ void APPS_UpdateData(uint32_t rawReading1, uint32_t rawReading2) {
 }
 
 float APPS_GetAPPSReading() {
-    return (appsData.appsReading1_Percentage + appsData.appsReading2_Percentage) / 2;
+    return (appsData.appsReading1_Percentage +
+            appsData.appsReading2_Percentage) /
+           2.0;
 }
 
-float APPS_GetAPPSReading1() {
-    return appsData.appsReading1_Percentage;
-}
+float APPS_GetAPPSReading1() { return appsData.appsReading1_Percentage; }
 
-float APPS_GetAPPSReading2() {
-    return appsData.appsReading2_Percentage;
-}
+float APPS_GetAPPSReading2() { return appsData.appsReading2_Percentage; }
 
 static void checkAndHandleAPPSFault() {
     // Check for open/short circuit
-    float difference = abs(appsData.appsReading1_Percentage - appsData.appsReading2_Percentage);
-
-    # if DEBUG_FLAG
-        Serial.print("Difference is: ");
-        Serial.println(difference);
-        Serial.print("Percent APPS1: ");
-        Serial.println(appsData.appsReading1_Percentage);
-        Serial.print("Percent APPS2: ");
-        Serial.println(appsData.appsReading2_Percentage);
-    # endif
-
-    if(appsData.appsReading1_Voltage < APPS_3V3_FAULT_MIN ||
-       appsData.appsReading1_Voltage > APPS_3V3_FAULT_MAX ||
-       appsData.appsReading2_Voltage < APPS_5V_FAULT_MIN ||
-       appsData.appsReading2_Voltage > APPS_5V_FAULT_MAX) {
+    float difference = abs(appsData.appsReading1_Percentage -
+                           appsData.appsReading2_Percentage);
+
+    // # if DEBUG_FLAG
+    Serial.print("Difference is: ");
+    Serial.println(difference);
+    Serial.print("Percent APPS1: ");
+    Serial.println(appsData.appsReading1_Percentage);
+    Serial.print("Percent APPS2: ");
+    Serial.println(appsData.appsReading2_Percentage);
+
+    // # endif
+
+    if (appsData.appsReading1_Voltage < APPS_3V3_FAULT_MIN ||
+        appsData.appsReading1_Voltage > APPS_3V3_FAULT_MAX ||
+        appsData.appsReading2_Voltage < APPS_5V_FAULT_MIN ||
+        appsData.appsReading2_Voltage > APPS_5V_FAULT_MAX) {
+
         TickType_t now = xTaskGetTickCount();
         TickType_t elapsedTicks = now - appsLatestHealthyStateTime;
         TickType_t elapsedMs = elapsedTicks * portTICK_PERIOD_MS;
-        if (elapsedMs > APPS_FAULT_TIME_THRESHOLD_MS){
-            # if DEBUG_FLAG
-                Serial.println("Setting APPS fault");
-            # endif
+
+        if (elapsedMs > APPS_FAULT_TIME_THRESHOLD_MS) {
+#if DEBUG_FLAG
+            Serial.println("Setting APPS fault");
+#endif
             Faults_SetFault(FAULT_APPS);
             return;
         }
@@ -116,9 +178,9 @@ static void checkAndHandleAPPSFault() {
         Faults_SetFault(FAULT_APPS);
         return;
     } else {
-        # if DEBUG_FLAG
-            Serial.println("Clearing fault in handle");
-        # endif
+#if DEBUG_FLAG
+        Serial.println("Clearing fault in handle");
+#endif
         Faults_ClearFault(FAULT_APPS);
     }
 }
@@ -128,20 +190,20 @@ static void checkAndHandlePlausibilityFault() {
     float BSEReading_Rear = BSE_GetBSEReading()->bseRear_PSI;
 
     float BSEReading = BSEReading_Front;
-    if (BSEReading_Rear > BSEReading_Front){
+    if (BSEReading_Rear > BSEReading_Front) {
         BSEReading = BSEReading_Rear;
     }
 
-    # if DEBUG_FLAG
-        Serial.print("BSE Reading: ");
-        Serial.println(BSEReading);
-    # endif
+#if DEBUG_FLAG
+    Serial.print("BSE Reading: ");
+    Serial.println(BSEReading);
+#endif
 
     if (APPS_GetAPPSReading() > APPS_BSE_PLAUSABILITY_TROTTLE_THRESHOLD &&
         BSEReading > APPS_BSE_PLAUSABILITY_BRAKE_THRESHOLD) {
         Faults_SetFault(FAULT_APPS_BRAKE_PLAUSIBILITY);
     } else {
-        if (APPS_GetAPPSReading() < APPS_BSE_PLAUSIBILITY_RESET_THRESHOLD){
+        if (APPS_GetAPPSReading() < APPS_BSE_PLAUSIBILITY_RESET_THRESHOLD) {
             Faults_ClearFault(FAULT_APPS_BRAKE_PLAUSIBILITY);
         }
     }
diff --git a/fsae-vehicle-fw/src/vehicle/apps.h b/fsae-vehicle-fw/src/vehicle/apps.h
index aad7e58..5f69b02 100644
--- a/fsae-vehicle-fw/src/vehicle/apps.h
+++ b/fsae-vehicle-fw/src/vehicle/apps.h
@@ -3,7 +3,7 @@
 #include "bse.h"
 
 void APPS_Init();
-void APPS_UpdateData(uint32_t rawReading1, uint32_t rawReading2);
+void APPS_UpdateData(uint16_t rawReading1, uint16_t rawReading2);
 float APPS_GetAPPSReading();
 float APPS_GetAPPSReading1();
 float APPS_GetAPPSReading2();
diff --git a/fsae-vehicle-fw/src/vehicle/bse.cpp b/fsae-vehicle-fw/src/vehicle/bse.cpp
index 637d5ec..7c78980 100644
--- a/fsae-vehicle-fw/src/vehicle/bse.cpp
+++ b/fsae-vehicle-fw/src/vehicle/bse.cpp
@@ -9,7 +9,7 @@
 #include "vehicle/faults.h"
 #include <arduino_freertos.h>
 
-typedef struct{
+typedef struct {
     float bseRawFront;
     float bseRawRear;
 } BSERawData;
@@ -17,8 +17,8 @@ typedef struct{
 static BSEData bseData;
 static BSERawData bseRawData;
 static float bseAlpha;
-static TickType_t bseLatestHealthyStateTime = 0; // Set to 0 when fault not detected
-
+static TickType_t bseLatestHealthyStateTime =
+    0; // Set to 0 when fault not detected
 
 void BSE_Init() {
     bseData.bseFront_PSI = 0;
@@ -27,10 +27,11 @@ void BSE_Init() {
     bseRawData.bseRawFront = 0;
     bseRawData.bseRawRear = 0;
 
-    bseAlpha = COMPUTE_ALPHA(BSE_CUTOFF_HZ); // 10Hz cutoff frequency, 0.01s sample time
+    bseAlpha = COMPUTE_ALPHA(
+        BSE_CUTOFF_HZ); // 10Hz cutoff frequency, 0.01s sample time
 }
 
-void BSE_UpdateData(uint32_t bseReading1, uint32_t bseReading2){
+void BSE_UpdateData(uint32_t bseReading1, uint32_t bseReading2) {
     // Filter incoming values
     LOWPASS_FILTER(bseReading1, bseRawData.bseRawFront, bseAlpha);
     LOWPASS_FILTER(bseReading2, bseRawData.bseRawRear, bseAlpha);
@@ -38,23 +39,23 @@ void BSE_UpdateData(uint32_t bseReading1, uint32_t bseReading2){
     float bseVoltage1 = ADC_VALUE_TO_VOLTAGE(bseRawData.bseRawFront);
     float bseVoltage2 = ADC_VALUE_TO_VOLTAGE(bseRawData.bseRawRear);
 
-    # if DEBUG_FLAG
-        Serial.print("BSE Voltage: ");
-        Serial.println(bseVoltage1);
-    # endif
+#if DEBUG_FLAG
+    Serial.print("BSE Voltage: ");
+    Serial.println(bseVoltage1);
+#endif
 
     // Check BSE open/short circuit
-    if(bseVoltage1 < BSE_LOWER_THRESHOLD ||
-       bseVoltage1 > BSE_UPPER_THRESHOLD ||
-       bseVoltage2 < BSE_LOWER_THRESHOLD ||
-       bseVoltage2 > BSE_UPPER_THRESHOLD) {
+    if (bseVoltage1 < BSE_LOWER_THRESHOLD ||
+        bseVoltage1 > BSE_UPPER_THRESHOLD ||
+        bseVoltage2 < BSE_LOWER_THRESHOLD ||
+        bseVoltage2 > BSE_UPPER_THRESHOLD) {
         TickType_t now = xTaskGetTickCount();
         TickType_t elapsedTicks = now - bseLatestHealthyStateTime;
         TickType_t elapsedMs = elapsedTicks * portTICK_PERIOD_MS;
-        if (elapsedMs > BSE_FAULT_TIME_THRESHOLD_MS){
-            # if DEBUG_FLAG
-                Serial.println("Setting BSE fault");
-            # endif
+        if (elapsedMs > BSE_FAULT_TIME_THRESHOLD_MS) {
+#if DEBUG_FLAG
+            Serial.println("Setting BSE fault");
+#endif
             Faults_SetFault(FAULT_BSE);
         }
 
@@ -67,6 +68,4 @@ void BSE_UpdateData(uint32_t bseReading1, uint32_t bseReading2){
     bseData.bseRear_PSI = BSE_VOLTAGE_TO_PSI(bseVoltage2);
 }
 
-BSEData* BSE_GetBSEReading() {
-    return &bseData;
-}
+BSEData *BSE_GetBSEReading() { return &bseData; }
diff --git a/fsae-vehicle-fw/src/vehicle/bse.h b/fsae-vehicle-fw/src/vehicle/bse.h
index 0a343e7..66444ba 100644
--- a/fsae-vehicle-fw/src/vehicle/bse.h
+++ b/fsae-vehicle-fw/src/vehicle/bse.h
@@ -5,10 +5,9 @@
 
 typedef struct {
     float bseFront_PSI; // front brake pressure in PSI
-    float bseRear_PSI; // rear brake pressure in PSI
+    float bseRear_PSI;  // rear brake pressure in PSI
 } BSEData;
 
 void BSE_Init();
 void BSE_UpdateData(uint32_t bseReading1, uint32_t bseReading2);
-BSEData* BSE_GetBSEReading();
-
+BSEData *BSE_GetBSEReading();
diff --git a/fsae-vehicle-fw/src/vehicle/faults.cpp b/fsae-vehicle-fw/src/vehicle/faults.cpp
index 83a8d05..daab0b5 100644
--- a/fsae-vehicle-fw/src/vehicle/faults.cpp
+++ b/fsae-vehicle-fw/src/vehicle/faults.cpp
@@ -1,5 +1,6 @@
 // Anteater Electric Racing, 2025
 
+
 #define FAULT_OVER_CURRENT_MASK (0x1)
 #define FAULT_UNDER_VOLTAGE_MASK (0x1 << 1)
 #define FAULT_OVER_TEMP_MASK (0x1 << 2)
@@ -8,68 +9,137 @@
 #define FAULT_BPPS_MASK (0x1 << 5)
 #define FAULT_APPS_BRAKE_PLAUSIBILITY_MASK (0x1 << 6)
 
+//Inverter Faults
+#define FAULT_DC_OVER_VOLT_FAULT_MASK (0x1 << 7)
+#define FAULT_MOTOR_PHASE_CURR_FAULT_MASK (0x1 << 8)
+#define FAULT_MCU_OVER_HOT_FAULT_MASK (0x1 << 9)
+#define FAULT_RESOLVER_FAULT_MASK (0x1 << 10)
+#define FAULT_PHASE_CURR_SENSOR_FAULT_MASK (0x1 << 11)
+#define FAULT_MOTOR_OVER_SPD_FAULT_MASK (0x1 << 12)
+#define FAULT_DRV_MOTOR_OVER_HOT_FAULT_MASK (0x1 << 13)
+#define FAULT_DC_MAIN_WIRE_OVER_CURR_FAULT_MASK (0x1 << 14)
+#define FAULT_DRV_MOTOR_OVER_COOL_FAULT_MASK (0x1 << 15)
+#define FAULT_DC_LOW_VOLT_WARNING_MASK (0x1 << 16)
+#define FAULT_MCU_12V_LOW_VOLT_WARNING_MASK (0x1 << 17)
+#define FAULT_MOTOR_STALL_FAULT_MASK (0x1 << 18)
+#define FAULT_MOTOR_OPEN_PHASE_FAULT_MASK (0x1 << 19)
+
+
+
 #include "vehicle/faults.h"
-#include "vehicle/motor.h"
 #include "utils/utils.h"
+#include "vehicle/motor.h"
 
-# if DEBUG_FLAG
-    #include <Arduino.h>
-# endif
+#if DEBUG_FLAG
+#include <Arduino.h>
+#endif
 
 static uint32_t faultBitMap;
 
-void Faults_Init() {
-    faultBitMap = 0;
-    }
+void Faults_Init() { faultBitMap = 0; }
 
 void Faults_SetFault(FaultType fault) {
     switch (fault) {
-        case FAULT_NONE: {
-            break;
-        }
-        case FAULT_OVER_CURRENT: {
-            faultBitMap |= FAULT_OVER_CURRENT_MASK;
-            break;
-        }
-        case FAULT_UNDER_VOLTAGE: {
-            faultBitMap |= FAULT_UNDER_VOLTAGE_MASK;
-            break;
-        }
-        case FAULT_OVER_TEMP: {
-            faultBitMap |= FAULT_OVER_TEMP_MASK;
-            break;
-        }
-        case FAULT_APPS: {
-            # if DEBUG_FLAG
-                Serial.println("Setting APPS fault");
-            # endif
-            faultBitMap |= FAULT_APPS_MASK;
-            break;
-        }
-        case FAULT_BSE: {
-            # if DEBUG_FLAG
-                Serial.println("Setting BSE fault");
-            # endif
-            faultBitMap |= FAULT_BSE_MASK;
-            break;
-        }
-        case FAULT_BPPS: {
-            faultBitMap |= FAULT_BPPS_MASK;
-            break;
-        }
-        case FAULT_APPS_BRAKE_PLAUSIBILITY: {
-            # if DEBUG_FLAG
-                Serial.println("Setting APPS Plausibility fault");
-            # endif
-            faultBitMap |= FAULT_APPS_BRAKE_PLAUSIBILITY_MASK;
-            break;
-        }
-        default: {
-            break;
-        }
+    case FAULT_NONE: {
+        break;
+    }
+    case FAULT_OVER_CURRENT: {
+        faultBitMap |= FAULT_OVER_CURRENT_MASK;
+        break;
+    }
+    case FAULT_UNDER_VOLTAGE: {
+        faultBitMap |= FAULT_UNDER_VOLTAGE_MASK;
+        break;
+    }
+    case FAULT_OVER_TEMP: {
+        faultBitMap |= FAULT_OVER_TEMP_MASK;
+        break;
+    }
+    case FAULT_APPS: {
+#if DEBUG_FLAG
+        Serial.println("Setting APPS fault");
+#endif
+        faultBitMap |= FAULT_APPS_MASK;
+        break;
+    }
+    case FAULT_BSE: {
+#if DEBUG_FLAG
+        Serial.println("Setting BSE fault");
+#endif
+        faultBitMap |= FAULT_BSE_MASK;
+        break;
+    }
+    case FAULT_BPPS: {
+        faultBitMap |= FAULT_BPPS_MASK;
+        break;
+    }
+    case FAULT_APPS_BRAKE_PLAUSIBILITY: {
+#if DEBUG_FLAG
+        Serial.println("Setting APPS Plausibility fault");
+#endif
+        faultBitMap |= FAULT_APPS_BRAKE_PLAUSIBILITY_MASK;
+        break;
     }
-}
 
+    // Inverter Faults
+    case FAULT_DC_OVER_VOLT: {
+        faultBitMap |= FAULT_DC_OVER_VOLT_FAULT_MASK;
+        break;
+    }
+    case FAULT_MOTOR_PHASE_CURR: {
+        faultBitMap |= FAULT_MOTOR_PHASE_CURR_FAULT_MASK;
+        break;
+    }
+    case FAULT_OVER_HOT: {
+        faultBitMap |= FAULT_MCU_OVER_HOT_FAULT_MASK;
+        break;
+    }
+    case FAULT_RESOLVER: {
+        faultBitMap |= FAULT_RESOLVER_FAULT_MASK;
+        break;
+    }
+    case FAULT_PHASE_CURR_SENSOR: {
+        faultBitMap |= FAULT_PHASE_CURR_SENSOR_FAULT_MASK;
+        break;
+    }
+    case FAULT_MOTOR_OVER_SPEED: {
+        faultBitMap |= FAULT_MOTOR_OVER_SPD_FAULT_MASK;
+        break;
+    }
+    case FAULT_DRV_OVER_HOT: {
+        faultBitMap |= FAULT_DRV_MOTOR_OVER_HOT_FAULT_MASK;
+        break;
+    }
+    case FAULT_DC_OVER_CURR: {
+        faultBitMap |= FAULT_DC_MAIN_WIRE_OVER_CURR_FAULT_MASK;
+        break;
+    }
+    case FAULT_DRV_OVER_COOL: {
+        faultBitMap |= FAULT_DRV_MOTOR_OVER_COOL_FAULT_MASK;
+        break;
+    }
+    case FAULT_DC_LOW_VOLT_WARN: {
+        faultBitMap |= FAULT_DC_LOW_VOLT_WARNING_MASK;
+        break;
+    }
+    case FAULT_12V_LOW_VOLT_WARN: {
+        faultBitMap |= FAULT_MCU_12V_LOW_VOLT_WARNING_MASK;
+        break;
+    }
+    case FAULT_MOTOR_STALL: {
+        faultBitMap |= FAULT_MOTOR_STALL_FAULT_MASK;
+        break;
+    }
+    case FAULT_MOTOR_OPEN_PHASE: {
+        faultBitMap |= FAULT_MOTOR_OPEN_PHASE_FAULT_MASK;
+        break;
+    }
+
+    default: {
+        break;
+    }
+    }
+}
 void Faults_ClearFault(FaultType fault) {
     switch (fault) {
     case FAULT_NONE: {
@@ -88,9 +158,9 @@ void Faults_ClearFault(FaultType fault) {
         break;
     }
     case FAULT_APPS: {
-        # if DEBUG_FLAG
-            Serial.println("Clearing APPS fault");
-        # endif
+#if DEBUG_FLAG
+        Serial.println("Clearing APPS fault");
+#endif
         faultBitMap &= ~FAULT_APPS_MASK;
         break;
     }
@@ -103,12 +173,67 @@ void Faults_ClearFault(FaultType fault) {
         break;
     }
     case FAULT_APPS_BRAKE_PLAUSIBILITY: {
-        # if DEBUG_FLAG
-            Serial.println("Clearing APPS BSE plausibility fault");
-        # endif
+#if DEBUG_FLAG
+        Serial.println("Clearing APPS BSE plausibility fault");
+#endif
         faultBitMap &= ~FAULT_APPS_BRAKE_PLAUSIBILITY_MASK;
         break;
     }
+
+    // Inverter Faults
+    case FAULT_DC_OVER_VOLT: {
+        faultBitMap &= ~FAULT_DC_OVER_VOLT_FAULT_MASK;
+        break;
+    }
+    case FAULT_MOTOR_PHASE_CURR: {
+        faultBitMap &= ~FAULT_MOTOR_PHASE_CURR_FAULT_MASK;
+        break;
+    }
+    case FAULT_OVER_HOT: {
+        faultBitMap &= ~FAULT_MCU_OVER_HOT_FAULT_MASK;
+        break;
+    }
+    case FAULT_RESOLVER: {
+        faultBitMap &= ~FAULT_RESOLVER_FAULT_MASK;
+        break;
+    }
+    case FAULT_PHASE_CURR_SENSOR: {
+        faultBitMap &= ~FAULT_PHASE_CURR_SENSOR_FAULT_MASK;
+        break;
+    }
+    case FAULT_MOTOR_OVER_SPEED: {
+        faultBitMap &= ~FAULT_MOTOR_OVER_SPD_FAULT_MASK;
+        break;
+    }
+    case FAULT_DRV_OVER_HOT: {
+        faultBitMap &= ~FAULT_DRV_MOTOR_OVER_HOT_FAULT_MASK;
+        break;
+    }
+    case FAULT_DC_OVER_CURR: {
+        faultBitMap &= ~FAULT_DC_MAIN_WIRE_OVER_CURR_FAULT_MASK;
+        break;
+    }
+    case FAULT_DRV_OVER_COOL: {
+        faultBitMap &= ~FAULT_DRV_MOTOR_OVER_COOL_FAULT_MASK;
+        break;
+    }
+    case FAULT_DC_LOW_VOLT_WARN: {
+        faultBitMap &= ~FAULT_DC_LOW_VOLT_WARNING_MASK;
+        break;
+    }
+    case FAULT_12V_LOW_VOLT_WARN: {
+        faultBitMap &= ~FAULT_MCU_12V_LOW_VOLT_WARNING_MASK;
+        break;
+    }
+    case FAULT_MOTOR_STALL: {
+        faultBitMap &= ~FAULT_MOTOR_STALL_FAULT_MASK;
+        break;
+    }
+    case FAULT_MOTOR_OPEN_PHASE: {
+        faultBitMap &= ~FAULT_MOTOR_OPEN_PHASE_FAULT_MASK;
+        break;
+    }
+
     default: {
         break;
     }
@@ -118,19 +243,20 @@ void Faults_ClearFault(FaultType fault) {
 // currently having all faults being handled the same but leaving room for
 // future customization
 void Faults_HandleFaults() {
-    # if DEBUG_FLAG
-        Serial.print("Fault bitmap: ");
-        Serial.println(faultBitMap);
-    # endif
+#if DEBUG_FLAG
+    Serial.print("Fault bitmap: ");
+    Serial.println(faultBitMap);
+#endif
 
     if (faultBitMap == 0) {
-        # if DEBUG_FLAG
-            Serial.println("Clearing all faults in handle faults");
-        # endif
-
+#if DEBUG_FLAG
+        Serial.println("Clearing all faults in handle faults");
+#endif
         Motor_ClearFaultState();
         return;
     }
+
+
     if (faultBitMap & FAULT_OVER_CURRENT_MASK) {
         Motor_SetFaultState();
     }
@@ -152,6 +278,47 @@ void Faults_HandleFaults() {
     if (faultBitMap & FAULT_APPS_BRAKE_PLAUSIBILITY_MASK) {
         Motor_SetFaultState();
     }
+
+    // Inverter Faults
+    if (faultBitMap & FAULT_DC_OVER_VOLT_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_MOTOR_PHASE_CURR_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_MCU_OVER_HOT_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_RESOLVER_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_PHASE_CURR_SENSOR_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_MOTOR_OVER_SPD_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_DRV_MOTOR_OVER_HOT_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_DC_MAIN_WIRE_OVER_CURR_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_DRV_MOTOR_OVER_COOL_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_DC_LOW_VOLT_WARNING_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_MCU_12V_LOW_VOLT_WARNING_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_MOTOR_STALL_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
+    if (faultBitMap & FAULT_MOTOR_OPEN_PHASE_FAULT_MASK) {
+        Motor_SetFaultState();
+    }
 }
 
 // for telemetry
diff --git a/fsae-vehicle-fw/src/vehicle/faults.h b/fsae-vehicle-fw/src/vehicle/faults.h
index 7536742..d71b0e5 100644
--- a/fsae-vehicle-fw/src/vehicle/faults.h
+++ b/fsae-vehicle-fw/src/vehicle/faults.h
@@ -12,9 +12,28 @@ typedef enum {
     FAULT_APPS,
     FAULT_BSE,
     FAULT_BPPS,
-    FAULT_APPS_BRAKE_PLAUSIBILITY
+    FAULT_APPS_BRAKE_PLAUSIBILITY,
+
+    // Inverter Faults
+    FAULT_DC_OVER_VOLT,
+    FAULT_MOTOR_PHASE_CURR,
+    FAULT_OVER_HOT,
+    FAULT_RESOLVER,
+    FAULT_PHASE_CURR_SENSOR,
+    FAULT_MOTOR_OVER_SPEED,
+    FAULT_DRV_OVER_HOT,
+    FAULT_DC_OVER_CURR,
+    FAULT_DRV_OVER_COOL,
+    FAULT_DC_LOW_VOLT_WARN,
+    FAULT_12V_LOW_VOLT_WARN,
+    FAULT_MOTOR_STALL,
+    FAULT_MOTOR_OPEN_PHASE,
+
+
 } FaultType;
 
+
+
 void Faults_Init();
 
 void Faults_SetFault(FaultType fault);
@@ -22,4 +41,3 @@ uint32_t Faults_GetFaults();
 void Faults_ClearFault(FaultType fault);
 void Faults_HandleFaults();
 bool Faults_CheckAllClear();
-
diff --git a/fsae-vehicle-fw/src/vehicle/ifl100-36.cpp b/fsae-vehicle-fw/src/vehicle/ifl100-36.cpp
index f6ff8f0..1b7da1c 100644
--- a/fsae-vehicle-fw/src/vehicle/ifl100-36.cpp
+++ b/fsae-vehicle-fw/src/vehicle/ifl100-36.cpp
@@ -10,6 +10,8 @@
 
 #include "utils/utils.h"
 
+#include "pcc_receive.h"
+
 #define THREAD_MCU_STACK_SIZE 128
 #define THREAD_MCU_PRIORITY 1
 
@@ -23,143 +25,159 @@ static MCU3Data mcu3Data;
 
 void MCU_Init() {
     // Fill with reasonable dummy values
-    mcu1Data = {
-        .motorSpeed = 0.0F,
-        .motorTorque = 0.0F,
-        .maxMotorTorque = MOTOR_MAX_TORQUE, // Max torque in Nm
-        .maxMotorBrakeTorque = MOTOR_MAX_TORQUE, // Max brake torque in Nm
-        .motorDirection = DIRECTION_STANDBY,
-        .mcuMainState = STATE_STANDBY,
-        .mcuWorkMode = WORK_MODE_STANDBY
-    };
-
-    mcu2Data = {
-        .motorTemp = 25, // Default temperature in C
-        .mcuTemp = 25, // Default temperature in C
-        .dcMainWireOverVoltFault = false,
-        .motorPhaseCurrFault = false,
-        .mcuOverHotFault = false,
-        .resolverFault = false,
-        .phaseCurrSensorFault = false,
-        .motorOverSpdFault = false,
-        .drvMotorOverHotFault = false,
-        .dcMainWireOverCurrFault = false,
-        .drvMotorOverCoolFault = false,
-        .mcuMotorSystemState = false,
-        .mcuTempSensorState = false,
-        .motorTempSensorState = false,
-        .dcVoltSensorState = false,
-        .dcLowVoltWarning = false,
-        .mcu12VLowVoltWarning = false,
-        .motorStallFault = false,
-        .motorOpenPhaseFault = false,
-        .mcuWarningLevel = ERROR_NONE
-    };
+    mcu1Data = {.motorSpeed = 0.0F,
+                .motorTorque = 0.0F,
+                .maxMotorTorque = MOTOR_MAX_TORQUE, // Max torque in Nm
+                .maxMotorBrakeTorque =
+                    MOTOR_MAX_TORQUE, // Max brake torque in Nm
+                .motorDirection = DIRECTION_STANDBY,
+                .mcuMainState = STATE_STANDBY,
+                .mcuWorkMode = WORK_MODE_STANDBY};
+
+    mcu2Data = {.motorTemp = 25, // Default temperature in C
+                .mcuTemp = 25,   // Default temperature in C
+                .dcMainWireOverVoltFault = false,
+                .motorPhaseCurrFault = false,
+                .mcuOverHotFault = false,
+                .resolverFault = false,
+                .phaseCurrSensorFault = false,
+                .motorOverSpdFault = false,
+                .drvMotorOverHotFault = false,
+                .dcMainWireOverCurrFault = false,
+                .drvMotorOverCoolFault = false,
+                .mcuMotorSystemState = false,
+                .mcuTempSensorState = false,
+                .motorTempSensorState = false,
+                .dcVoltSensorState = false,
+                .dcLowVoltWarning = false,
+                .mcu12VLowVoltWarning = false,
+                .motorStallFault = false,
+                .motorOpenPhaseFault = false,
+                .mcuWarningLevel = ERROR_NONE};
 
     mcu3Data = {
-        .mcuVoltage = 0.0F, // Default voltage in V
-        .mcuCurrent = 0.0F, // Default current in A
-        .motorPhaseCurr = 0.0F // Default phase current in A
+        .mcuVoltage = 0.0F,    // Default voltage in V
+        .mcuCurrent = 16.0F,   // Default current in A
+        .motorPhaseCurr = 3.0F // Default phase current in A
     };
 
     // Initialize the motor thread
-    xTaskCreate(threadMCU, "threadMCU", THREAD_MCU_STACK_SIZE, NULL, THREAD_MCU_PRIORITY, NULL);
+    xTaskCreate(threadMCU, "threadMCU", THREAD_MCU_STACK_SIZE, NULL,
+                THREAD_MCU_PRIORITY, NULL);
 }
 
 static void threadMCU(void *pvParameters) {
     while (true) {
         // Read the CAN messages
         CAN_Receive(&rx_id, &rx_data);
-        switch(rx_id) {
-            case mMCU1_ID:
-            {
-                MCU1 mcu1 = {0};
-                memcpy(&mcu1, &rx_data, sizeof(mcu1));
-
-                int8_t torqueDirection = 0; // 1 if power drive state, -1 if generate electricity state
-                if (mcu1.MCU_MotorState == 1) torqueDirection = 1;
-                else if (mcu1.MCU_MotorState == 2) torqueDirection = -1;
-
-                taskENTER_CRITICAL(); // Enter critical section
-                mcu1Data = {
-                    .motorSpeed = CHANGE_ENDIANESS_16(mcu1.MCU_ActMotorSpd) * 0.25F, // convert to RPM
-                    .motorTorque = mcu1.MCU_ActMotorTq * 0.392F * torqueDirection * MOTOR_MAX_TORQUE, // convert to Nm
-                    .maxMotorTorque = mcu1.MCU_MaxMotorTq * 0.392F * MOTOR_MAX_TORQUE, // convert to Nm
-                    .maxMotorBrakeTorque = mcu1.MCU_MaxMotorBrakeTq * 0.392F * MOTOR_MAX_TORQUE, // convert to Nm
-                    .motorDirection = (MotorRotateDirection) mcu1.MCU_MotorRotateDirection,
-                    .mcuMainState = (MCUMainState) mcu1.MCU_MotorMainState,
-                    .mcuWorkMode = (MCUWorkMode) mcu1.MCU_MotorWorkMode
-                };
-                taskEXIT_CRITICAL(); // Exit critical section
-                break;
-            }
-            case mMCU2_ID:
-            {
-                MCU2 mcu2 = {0};
-                memcpy(&mcu2, &rx_data, sizeof(mcu2));
-
-                taskENTER_CRITICAL(); // Enter critical section
-                mcu2Data = {
-                    .motorTemp = mcu2.MCU_Motor_Temp - 40, // convert to C
-                    .mcuTemp = mcu2.MCU_hardwareTemp - 40, // convert to C
-                    .dcMainWireOverVoltFault = mcu2.MCU_DCMainWireOverVoltFault ? true : false,
-                    .motorPhaseCurrFault = mcu2.MCU_MotorPhaseCurrFault ? true : false,
-                    .mcuOverHotFault = mcu2.MCU_OverHotFault ? true : false,
-                    .resolverFault = mcu2.MCU_MotorResolver_Fault ? true : false,
-                    .phaseCurrSensorFault = mcu2.MCU_PhaseCurrSensorState ? true : false,
-                    .motorOverSpdFault = mcu2.MCU_MotorOverSpdFault ? true : false,
-                    .drvMotorOverHotFault = mcu2.Drv_MotorOverHotFault ? true : false,
-                    .dcMainWireOverCurrFault = mcu2.MCU_DCMainWireOverCurrFault ? true : false,
-                    .drvMotorOverCoolFault = mcu2.Drv_MotorOverCoolFault ? true : false,
-                    .mcuMotorSystemState = mcu2.MCU_MotorSystemState ? true : false,
-                    .mcuTempSensorState = mcu2.MCU_TempSensorState ? true : false,
-                    .motorTempSensorState = mcu2.MCU_MotorTempSensorState ? true : false,
-                    .dcVoltSensorState = mcu2.MCU_DC_VoltSensorState ? true : false,
-                    .dcLowVoltWarning = mcu2.MCU_DC_LowVoltWarning ? true : false,
-                    .mcu12VLowVoltWarning = mcu2.MCU_12V_LowVoltWarning ? true : false,
-                    .motorStallFault = mcu2.MCU_MotorStallFault ? true : false,
-                    .motorOpenPhaseFault = mcu2.MCU_MotorOpenPhaseFault ? true : false,
-                    .mcuWarningLevel = (MCUWarningLevel) mcu2.MCU_Warning_Level
-                };
-                taskEXIT_CRITICAL(); // Exit critical section
-                break;
-            }
-            case mMCU3_ID:
-            {
-                MCU3 mcu3 = {0};
-                memcpy(&mcu3, &rx_data, sizeof(mcu3));
-
-                taskENTER_CRITICAL(); // Enter critical section
-                mcu3Data = {
-                    .mcuVoltage = CHANGE_ENDIANESS_16(mcu3.MCU_DC_MainWireVolt) * 0.01F, // convert to V
-                    .mcuCurrent = CHANGE_ENDIANESS_16(mcu3.MCU_DC_MainWireCurr) * 0.01F, // convert to A
-                    .motorPhaseCurr = CHANGE_ENDIANESS_16(mcu3.MCU_MotorPhaseCurr) * 0.01F, // convert to A
-                };
-                taskEXIT_CRITICAL(); // Exit critical section
-                break;
-            }
-            default:
-            {
-                break;
-            }
+        switch (rx_id) {
+        case mMCU1_ID: {
+            MCU1 mcu1 = {0};
+            memcpy(&mcu1, &rx_data, sizeof(mcu1));
+
+            int8_t torqueDirection =
+                0; // 1 if power drive state, -1 if generate electricity state
+            if (mcu1.MCU_MotorState == 1)
+                torqueDirection = 1;
+            else if (mcu1.MCU_MotorState == 2)
+                torqueDirection = -1;
+
+            taskENTER_CRITICAL(); // Enter critical section
+            mcu1Data = {
+                .motorSpeed = CHANGE_ENDIANESS_16(mcu1.MCU_ActMotorSpd) *
+                              0.25F, // convert to RPM
+                .motorTorque = mcu1.MCU_ActMotorTq * 0.392F * torqueDirection *
+                               MOTOR_MAX_TORQUE, // convert to Nm
+                .maxMotorTorque = mcu1.MCU_MaxMotorTq * 0.392F *
+                                  MOTOR_MAX_TORQUE, // convert to Nm
+                .maxMotorBrakeTorque = mcu1.MCU_MaxMotorBrakeTq * 0.392F *
+                                       MOTOR_MAX_TORQUE, // convert to Nm
+                .motorDirection =
+                    (MotorRotateDirection)mcu1.MCU_MotorRotateDirection,
+                .mcuMainState = (MCUMainState)mcu1.MCU_MotorMainState,
+                .mcuWorkMode = (MCUWorkMode)mcu1.MCU_MotorWorkMode};
+            taskEXIT_CRITICAL(); // Exit critical section
+            break;
+        }
+        case mMCU2_ID: {
+            MCU2 mcu2 = {0};
+            memcpy(&mcu2, &rx_data, sizeof(mcu2));
+
+            taskENTER_CRITICAL(); // Enter critical section
+            mcu2Data = {
+                .motorTemp = mcu2.MCU_Motor_Temp - 40, // convert to C
+                .mcuTemp = mcu2.MCU_hardwareTemp - 40, // convert to C
+                .dcMainWireOverVoltFault =
+                    mcu2.MCU_DCMainWireOverVoltFault ? true : false,
+                .motorPhaseCurrFault =
+                    mcu2.MCU_MotorPhaseCurrFault ? true : false,
+                .mcuOverHotFault = mcu2.MCU_OverHotFault ? true : false,
+                .resolverFault = mcu2.MCU_MotorResolver_Fault ? true : false,
+                .phaseCurrSensorFault =
+                    mcu2.MCU_PhaseCurrSensorState ? true : false,
+                .motorOverSpdFault = mcu2.MCU_MotorOverSpdFault ? true : false,
+                .drvMotorOverHotFault =
+                    mcu2.Drv_MotorOverHotFault ? true : false,
+                .dcMainWireOverCurrFault =
+                    mcu2.MCU_DCMainWireOverCurrFault ? true : false,
+                .drvMotorOverCoolFault =
+                    mcu2.Drv_MotorOverCoolFault ? true : false,
+                .mcuMotorSystemState = mcu2.MCU_MotorSystemState ? true : false,
+                .mcuTempSensorState = mcu2.MCU_TempSensorState ? true : false,
+                .motorTempSensorState =
+                    mcu2.MCU_MotorTempSensorState ? true : false,
+                .dcVoltSensorState = mcu2.MCU_DC_VoltSensorState ? true : false,
+                .dcLowVoltWarning = mcu2.MCU_DC_LowVoltWarning ? true : false,
+                .mcu12VLowVoltWarning =
+                    mcu2.MCU_12V_LowVoltWarning ? true : false,
+                .motorStallFault = mcu2.MCU_MotorStallFault ? true : false,
+                .motorOpenPhaseFault =
+                    mcu2.MCU_MotorOpenPhaseFault ? true : false,
+                .mcuWarningLevel = (MCUWarningLevel)mcu2.MCU_Warning_Level};
+            taskEXIT_CRITICAL(); // Exit critical section
+            break;
+        }
+        case mMCU3_ID: {
+            MCU3 mcu3 = {0};
+            memcpy(&mcu3, &rx_data, sizeof(mcu3));
+
+            taskENTER_CRITICAL(); // Enter critical section
+
+            mcu3Data = {
+                .mcuVoltage = (((mcu3.MCU_DC_MainWireVolt & 0xFF) << 8) |
+                               (mcu3.MCU_DC_MainWireVolt >> 8)) *
+                              0.01F, // convert to V
+                .mcuCurrent = (((mcu3.MCU_DC_MainWireCurr & 0xFF) << 8) |
+                               (mcu3.MCU_DC_MainWireCurr >> 8)) *
+                              0.01F, // convert to A
+                .motorPhaseCurr = (((mcu3.MCU_MotorPhaseCurr & 0xFF) << 8) |
+                                   (mcu3.MCU_MotorPhaseCurr >> 8)) *
+                                  0.01F, // convert to A
+            };
+
+            taskEXIT_CRITICAL(); // Exit critical section
+            break;
+        }
+        case pcc_ID: {
+            // Serial.print("PCC OK?");
+            processPCCMessage(rx_data);
+            break;
+        }
+        default: {
+            break;
+        }
         }
     }
 }
 
-MCU1Data* MCU_GetMCU1Data() {
-    return &mcu1Data;
-}
+MCU1Data *MCU_GetMCU1Data() { return &mcu1Data; }
 
-MCU2Data* MCU_GetMCU2Data() {
-    return& mcu2Data;
-}
+MCU2Data *MCU_GetMCU2Data() { return &mcu2Data; }
 
-MCU3Data* MCU_GetMCU3Data() {
-    return& mcu3Data;
-}
+MCU3Data *MCU_GetMCU3Data() { return &mcu3Data; }
 
 // checksum = (byte0 + byte1 + byte2 + byte3 + byte4 + byte5 + byte6) XOR 0xFF
-uint8_t ComputeChecksum(uint8_t* data) {
+uint8_t ComputeChecksum(uint8_t *data) {
     uint8_t sum = 0;
     for (uint8_t i = 0; i < 7; i++) {
         sum += data[i];
diff --git a/fsae-vehicle-fw/src/vehicle/ifl100-36.h b/fsae-vehicle-fw/src/vehicle/ifl100-36.h
index b6fcc07..8fb681b 100644
--- a/fsae-vehicle-fw/src/vehicle/ifl100-36.h
+++ b/fsae-vehicle-fw/src/vehicle/ifl100-36.h
@@ -10,6 +10,7 @@
 #define mMCU1_ID 0x105
 #define mMCU2_ID 0x106
 #define mMCU3_ID 0x107
+#define pcc_ID 0x222
 
 #define mBMS1_ID 0x1A0
 #define mBMS2_ID 0x1A1
@@ -49,11 +50,11 @@ typedef struct __attribute__((packed)) {
 
     uint64_t Reserved : 5;
     uint64_t VCUAuthenticationStatus : 2;
-    uint64_t ChangeGearAlarm: 1; // end of byte 3
+    uint64_t ChangeGearAlarm : 1; // end of byte 3
 
-    uint64_t VehicleState: 1;
-    uint64_t Brake_Pedal_Sts: 2;
-    uint64_t BMS_Main_Relay_Cmd: 1;
+    uint64_t VehicleState : 1;
+    uint64_t Brake_Pedal_Sts : 2;
+    uint64_t BMS_Main_Relay_Cmd : 1;
     uint64_t GearLeverPos_Sts : 3;
     uint64_t GearLeverPos_Sts_F : 1; // end of byte 4
 
@@ -64,38 +65,38 @@ typedef struct __attribute__((packed)) {
     uint64_t KeyPosition : 2; // end of byte 5
 
     uint64_t RollingCounter : 4;
-    uint64_t BMS_Aux_Relay_Cmd: 1;
+    uint64_t BMS_Aux_Relay_Cmd : 1;
     uint64_t PowerReduceReq : 3; // end of byte 6
 
     uint64_t CheckSum : 8; // end of byte 7
-} VCU1; // bit order verified
+} VCU1;                    // bit order verified
 
 typedef struct __attribute__((packed)) {
-    uint64_t MCU_ActMotorSpd : 16; // end of byte 0 and byte 1
-    uint64_t MCU_ActMotorTq: 8; // end of byte 2
-    uint64_t MCU_MaxMotorTq : 8; // end of byte 3
+    uint64_t MCU_ActMotorSpd : 16;    // end of byte 0 and byte 1
+    uint64_t MCU_ActMotorTq : 8;      // end of byte 2
+    uint64_t MCU_MaxMotorTq : 8;      // end of byte 3
     uint64_t MCU_MaxMotorBrakeTq : 8; // end of byte 4
 
     // flipped
-    uint64_t Reserved: 3;
+    uint64_t Reserved : 3;
     uint64_t MCU_MotorMainState : 3;
     uint64_t MCU_MotorRotateDirection : 2; // end of byte 5
 
     uint64_t RollingCounter : 4; // end of byte 6
     uint64_t MCU_MotorState : 2;
-    uint64_t MCU_MotorWorkMode: 2;
+    uint64_t MCU_MotorWorkMode : 2;
 
     uint64_t CheckSum : 8; // end of byte 7
-} MCU1; // order fixed, needs to be verified
+} MCU1;                    // order fixed, needs to be verified
 
 typedef struct __attribute__((packed)) {
-    uint64_t MCU_Motor_Temp : 8; // end of byte 0
-    uint64_t MCU_hardwareTemp: 8; // end of byte 1
+    uint64_t MCU_Motor_Temp : 8;   // end of byte 0
+    uint64_t MCU_hardwareTemp : 8; // end of byte 1
 
     uint64_t MCU_DCMainWireOverCurrFault : 1;
     uint64_t MCU_MotorPhaseCurrFault : 1;
-    uint64_t MCU_OverHotFault: 1;
-    uint64_t MCU_MotorResolver_Fault: 1;
+    uint64_t MCU_OverHotFault : 1;
+    uint64_t MCU_MotorResolver_Fault : 1;
     uint64_t MCU_PhaseCurrSensorState : 1;
     uint64_t MCU_MotorOverSpdFault : 1;
     uint64_t Drv_MotorOverHotFault : 1;
@@ -116,25 +117,25 @@ typedef struct __attribute__((packed)) {
     uint64_t MCU_MotorStallFault : 1; // end of byte 6
 
     uint64_t CheckSum : 8; // end of byte 7
-} MCU2; // fixed order, needs to be verified
+} MCU2;                    // fixed order, needs to be verified
 
 typedef struct __attribute__((packed)) {
     uint64_t MCU_DC_MainWireVolt : 16; // end of byte 0 and byte 1
-    uint64_t MCU_DC_MainWireCurr: 16; // end of byte 2 and byte 3
-    uint64_t MCU_MotorPhaseCurr : 16; // end of byte 4 and byte 5
-    uint64_t Reserved : 16; // end of byte 6 and byte 7
-} MCU3; // bit order verified
+    uint64_t MCU_DC_MainWireCurr : 16; // end of byte 2 and byte 3
+    uint64_t MCU_MotorPhaseCurr : 16;  // end of byte 4 and byte 5
+    uint64_t Reserved : 16;            // end of byte 6 and byte 7
+} MCU3;                                // bit order verified
 
 typedef struct __attribute__((packed)) {
     uint64_t BMS_Warning_Level : 2;
     uint64_t Batt_charge_Sts : 2;
     uint64_t Negative_Relay_FB : 1;
     uint64_t Positive_Relay_FB : 1;
-    uint64_t HighVoltLoopLockSts: 1;
+    uint64_t HighVoltLoopLockSts : 1;
     uint64_t Batt_Charge_Sts_F : 1; // end of byte 0
 
-    uint64_t Batt_SOC_Value: 8; // end of byte 1
-    uint64_t Batt_SOH_Value: 8; // end of byte 2
+    uint64_t Batt_SOC_Value : 8; // end of byte 1
+    uint64_t Batt_SOH_Value : 8; // end of byte 2
 
     uint64_t BMS_Cmd_AC_DC : 2;
     uint64_t Pre_charge_Relay_FB : 1;
@@ -147,7 +148,7 @@ typedef struct __attribute__((packed)) {
     uint64_t Batt_Pack_Coincidence_Alarm : 1;
     uint64_t Batt_Pack_Matching_Alarm : 1;
     uint64_t Insulation_Resistance_Low_F : 1;
-    uint64_t HighVoltLoopLockSts_F: 1;
+    uint64_t HighVoltLoopLockSts_F : 1;
     uint64_t ChargeRelayAdhereDetectionSts : 1;
     uint64_t RelayAdhereDetectionSts : 1; // end of byte 4
 
@@ -157,62 +158,61 @@ typedef struct __attribute__((packed)) {
     uint64_t Reserved3 : 4; // end of byte 6
 
     uint64_t CheckSum : 8; // end of byte 7
-} BMS1; // bit order verified
+} BMS1;                    // bit order verified
 
 typedef struct __attribute__((packed)) {
-    uint64_t sAllowMaxDischarge : 16; // end of byte 1 and byte 2
-    uint64_t sAllowMaxRegenCharge: 16; // end of byte 3 and byte 4
+    uint64_t sAllowMaxDischarge : 16;   // end of byte 1 and byte 2
+    uint64_t sAllowMaxRegenCharge : 16; // end of byte 3 and byte 4
 
-    uint64_t Reserved : 20; // end of byte 5
+    uint64_t Reserved : 20;      // end of byte 5
     uint64_t RollingCounter : 4; // end of byte 6
 
     uint64_t CheckSum : 8; // end of byte 7
-} BMS2; // bit order verified
+} BMS2;                    // bit order verified
 
 typedef struct {
     // MCU1 data
-    float motorSpeed; // Motor speed in RPM
-    float motorTorque; // Motor torque in Nm
-    float maxMotorTorque; // Max motor torque in Nm
-    float maxMotorBrakeTorque; // Max motor brake torque in Nm
+    float motorSpeed;                    // Motor speed in RPM
+    float motorTorque;                   // Motor torque in Nm
+    float maxMotorTorque;                // Max motor torque in Nm
+    float maxMotorBrakeTorque;           // Max motor brake torque in Nm
     MotorRotateDirection motorDirection; // Motor direction
-    MCUMainState mcuMainState; // Motor main state
-    MCUWorkMode mcuWorkMode; // MCU work mode
+    MCUMainState mcuMainState;           // Motor main state
+    MCUWorkMode mcuWorkMode;             // MCU work mode
 } MCU1Data;
 
 typedef struct {
-    int32_t motorTemp; // Motor temperature in C
-    int32_t mcuTemp; // Hardware temperature in C
-    bool dcMainWireOverVoltFault; // DC over voltage fault
-    bool motorPhaseCurrFault; // MCU motor phase current fault
-    bool mcuOverHotFault; // MCU overheat fault
-    bool resolverFault; // Resolver fault
-    bool phaseCurrSensorFault; // Phase current sensor fault
-    bool motorOverSpdFault; // MCU motor over speed fault
-    bool drvMotorOverHotFault; // Driver motor overheat fault
-    bool dcMainWireOverCurrFault; // DC main wire over voltage fault
-    bool drvMotorOverCoolFault; // Driver motor overcool fault
-    bool mcuMotorSystemState; // MCU motor system state
-    bool mcuTempSensorState; // MCU temperature sensor state
-    bool motorTempSensorState; // MCU motor temperature sensor state
-    bool dcVoltSensorState; // MCU DC voltage sensor state
-    bool dcLowVoltWarning; // MCU DC low voltage warning
-    bool mcu12VLowVoltWarning; // MCU 12V low voltage warning
-    bool motorStallFault; // MCU motor stall fault
-    bool motorOpenPhaseFault; // MCU motor open phase fault
+    int32_t motorTemp;               // Motor temperature in C
+    int32_t mcuTemp;                 // Hardware temperature in C
+    bool dcMainWireOverVoltFault;    // DC over voltage fault
+    bool motorPhaseCurrFault;        // MCU motor phase current fault
+    bool mcuOverHotFault;            // MCU overheat fault
+    bool resolverFault;              // Resolver fault
+    bool phaseCurrSensorFault;       // Phase current sensor fault
+    bool motorOverSpdFault;          // MCU motor over speed fault
+    bool drvMotorOverHotFault;       // Driver motor overheat fault
+    bool dcMainWireOverCurrFault;    // DC main wire over voltage fault
+    bool drvMotorOverCoolFault;      // Driver motor overcool fault
+    bool mcuMotorSystemState;        // MCU motor system state
+    bool mcuTempSensorState;         // MCU temperature sensor state
+    bool motorTempSensorState;       // MCU motor temperature sensor state
+    bool dcVoltSensorState;          // MCU DC voltage sensor state
+    bool dcLowVoltWarning;           // MCU DC low voltage warning
+    bool mcu12VLowVoltWarning;       // MCU 12V low voltage warning
+    bool motorStallFault;            // MCU motor stall fault
+    bool motorOpenPhaseFault;        // MCU motor open phase fault
     MCUWarningLevel mcuWarningLevel; // MCU warning level
 } MCU2Data;
 
 typedef struct {
-    float mcuVoltage; // DC main wire voltage in V
-    float mcuCurrent; // DC main wire current in A
+    float mcuVoltage;     // DC main wire voltage in V
+    float mcuCurrent;     // DC main wire current in A
     float motorPhaseCurr; // Motor phase current in A
 } MCU3Data;
 
 void MCU_Init();
-uint8_t ComputeChecksum(uint8_t* data);
+uint8_t ComputeChecksum(uint8_t *data);
 
-
-MCU1Data* MCU_GetMCU1Data();
-MCU2Data* MCU_GetMCU2Data();
-MCU3Data* MCU_GetMCU3Data();
+MCU1Data *MCU_GetMCU1Data();
+MCU2Data *MCU_GetMCU2Data();
+MCU3Data *MCU_GetMCU3Data();
diff --git a/fsae-vehicle-fw/src/vehicle/motor.cpp b/fsae-vehicle-fw/src/vehicle/motor.cpp
index 6c42ae8..cf04ab4 100644
--- a/fsae-vehicle-fw/src/vehicle/motor.cpp
+++ b/fsae-vehicle-fw/src/vehicle/motor.cpp
@@ -1,23 +1,24 @@
 // Anteater Electric Racing, 2025
 
-
 #define SPEED_CONTROL_ENABLED 0
 #define SPEED_P_GAIN 0.01F // Proportional gain for speed control
-#define SPEED_I_GAIN 0.1F // Integral gain for speed control
+#define SPEED_I_GAIN 0.1F  // Integral gain for speed control
 
 #include <arduino_freertos.h>
 
 #include "utils/utils.h"
 
 #include "peripherals/can.h"
+#include "peripherals/gpio.h"
 
-#include "vehicle/motor.h"
-#include "vehicle/telemetry.h"
-#include "vehicle/rtm_button.h"
 #include "apps.h"
 #include "vehicle/ifl100-36.h"
+#include "vehicle/motor.h"
+#include "vehicle/pcc_receive.h"
+#include "vehicle/rtm_button.h"
+#include "vehicle/telemetry.h"
 
-typedef struct{
+typedef struct {
     MotorState state;
     float desiredTorque; // Torque demand in Nm;
 } MotorData;
@@ -28,79 +29,146 @@ static VCU1 vcu1 = {0};
 static BMS1 bms1 = {0};
 static BMS2 bms2 = {0};
 
-void Motor_Init(){
-    motorData.state = MOTOR_STATE_OFF; // TODO Check if we want this
-    motorData.desiredTorque = 0.0F; // No torque demand at start
+/**
+ *
+ * TODO 10/24/25
+ *
+ * CAN Communication from PCC (like ifl100-36)
+ *
+ * TODO Future: Enums for VCU transitions or abstracted T1-T8 transitions
+ *
+ */
+
+void Motor_Init() {
+    motorData.state = MOTOR_STATE_PRECHARGING; // TODO Check if we want this
+    motorData.desiredTorque = 0.0F;            // No torque demand at start
 }
 
-void threadMotor(void *pvParameters){
-    while(true){
+void threadMotor(void *pvParameters) {
+    while (true) {
         // Clear packet contents
         vcu1 = {0};
         bms1 = {0};
         bms2 = {0};
-        switch (motorData.state){
-            case MOTOR_STATE_OFF:
-            {
-                break;
-            }
-            case MOTOR_STATE_PRECHARGING:
-            {
-                // T2 State transition: BMS_Main_Relay_Cmd == 1 && Pre_charge_Relay_FB == 1
-                vcu1.BMS_Main_Relay_Cmd = 1; // 1 = ON, 0 = OFF
-                bms1.Pre_charge_Relay_FB = 1; // 1 = ON, 0 = OFF
-                break;
-            }
-            case MOTOR_STATE_IDLE:
-            {
-                // T4 BMS_Main_Relay_Cmd == 1 && Pre_charge_Finish_Sts == 1 && Ubat>=200V
-                vcu1.BMS_Main_Relay_Cmd = 1; // 1 = ON, 0 = OFF
-                bms1.Pre_charge_Relay_FB = 1; // 1 = ON, 0 = OFF NOTE: see if we can omit this bit
-                bms1.Pre_charge_Finish_Sts = 1; // 1 = ON, 0 = OFF
-                break;
-            }
-            case MOTOR_STATE_DRIVING:
-            {
-                uint16_t maxDischarge = (uint16_t) (BATTERY_MAX_CURRENT_A + 500) * 10;
-                uint16_t maxRegen = (uint16_t) (BATTERY_MAX_REGEN_A + 500) * 10;
-
-                bms2.sAllowMaxDischarge = CHANGE_ENDIANESS_16(maxDischarge); // Convert to little-endian format
-                bms2.sAllowMaxRegenCharge = CHANGE_ENDIANESS_16(maxRegen); // Convert to little-endian format
-
-                // T5 BMS_Main_Relay_Cmd == 1 && VCU_MotorMode = 1/2
-                vcu1.BMS_Main_Relay_Cmd = 1; // 1 = ON, 0 = OFF
-                bms1.Pre_charge_Relay_FB = 1; // 1 = ON, 0 = OFF NOTE: see if we can omit this bit
-                bms1.Pre_charge_Finish_Sts = 1; // 1 = ON, 0 = OFF
-
-                vcu1.VehicleState = 1; // 0 = Not ready, 1 = Ready
-                vcu1.GearLeverPos_Sts = 3; // 0 = Default, 1 = R, 2 = N, 3 = D, 4 = P
-                vcu1.AC_Control_Cmd = 1; // 0 = Not active, 1 = Active
-                vcu1.BMS_Aux_Relay_Cmd = 1; // 0 = not work, 1 = work
-                vcu1.KeyPosition = 2; // 0 = Off, 1 = ACC, 2 = ON, 2 = Crank+On
-
-                vcu1.VCU_TorqueReq = (uint8_t) ((fabsf(motorData.desiredTorque) / MOTOR_MAX_TORQUE) * 100); // Torque demand in percentage (0-99.6) 350Nm
-                vcu1.VCU_MotorMode = motorData.desiredTorque >= 0 ? 1 : 2; // 0 = Standby, 1 = Drive, 2 = Generate Electricy, 3 = Reserved
-                break;
-            }
-            case MOTOR_STATE_FAULT:
-            {
-                // T7 MCU_Warning_Level == 3
-                vcu1.BMS_Main_Relay_Cmd = 1; // 1 = ON, 0 = OFF
-                bms1.Pre_charge_Relay_FB = 1; // 1 = ON, 0 = OFF NOTE: see if we can omit this bit
-                bms1.Pre_charge_Finish_Sts = 1; // 1 = ON, 0 = OFF
-                vcu1.VCU_Warning_Level = 1; // 0 = No Warning, 1 = Warning, 2 = Fault, 3 = Critical Fault
-                vcu1.VCU_MotorMode = 0; // 0 = Standby, 1 = Drive, 2 = Generate Electricy, 3 = Reserved
-                break;
-            }
-            default:
-            {
-                break;
-            }
+
+        /*
+        if (!CAN_IsBusHealthy(2) || !CAN_IsBusHealthy(3)) {
+            // CAN is lost, enter fault mode immediately
+            motorData.state = MOTOR_STATE_FAULT;
+            motorData.desiredTorque = 0.0F;
+
+            #if DEBUG_FLAG
+                Serial.println("CAN fault detected — entering FAULT state");
+            #endif
+        }*/
+
+        // TODO: Check if keyposition does anything --> if not then manually
+        // switch kl15, and provision a gpio pin to control a 3.3v switch or
+        // mosfet
+        switch (motorData.state) {
+
+        // TODO: MAKE this state 4? --> matches with inverter state machine
+        case MOTOR_STATE_OFF: {
+            // T8
+            // vcu1.KeyPosition = 0;
+            break;
+        }
+
+        case MOTOR_STATE_STANDBY: {
+            // T1
+            // vcu1.KeyPosition = 2;
+
+            // T3
+            vcu1.BMS_Main_Relay_Cmd = 0;
+            bms1.Pre_charge_Relay_FB =
+                0; // 1 = ON, 0 = OFF NOTE: see if we can omit this bit
+            bms1.Pre_charge_Finish_Sts = 0;
+            break;
+        }
+
+        case MOTOR_STATE_PRECHARGING: {
+            // vcu1.KeyPosition = 2;
+            //  T2 State transition: BMS_Main_Relay_Cmd == 1 &&
+            //  Pre_charge_Relay_FB == 1
+            vcu1.BMS_Main_Relay_Cmd = 1;  // 1 = ON, 0 = OFF
+            bms1.Pre_charge_Relay_FB = 1; // 1 = ON, 0 = OFF
+            vcu1.VCU_TorqueReq = 0;
+            break;
+        }
+
+        case MOTOR_STATE_IDLE: {
+            // T4 BMS_Main_Relay_Cmd == 1 && Pre_charge_Finish_Sts == 1 &&
+            // Ubat>=200V
+            vcu1.BMS_Main_Relay_Cmd = 1; // 1 = ON, 0 = OFF
+            bms1.Pre_charge_Relay_FB =
+                1; // 1 = ON, 0 = OFF NOTE: see if we can omit this bit
+            bms1.Pre_charge_Finish_Sts = 1; // 1 = ON, 0 = OFF
+
+            // T6
+            vcu1.VCU_MotorMode = 0;
+            vcu1.VCU_TorqueReq = 0;
+            // Convert to little-endian format
+            // bms2.sAllowMaxRegenCharge = CHANGE_ENDIANESS_16(maxRegen); //
+            // Convert to little-endian format
+
+            break;
+        }
+
+        case MOTOR_STATE_DRIVING: {
+
+            uint16_t maxDischarge =
+                (uint16_t)(BATTERY_MAX_CURRENT_A + 500) * 10;
+            // uint16_t maxRegen = (uint16_t)(BATTERY_MAX_REGEN_A + 500) * 10;
+            bms2.sAllowMaxDischarge = CHANGE_ENDIANESS_16(maxDischarge);
+            // bms2.sAllowMaxRegenCharge = CHANGE_ENDIANESS_16(
+            //     maxRegen); // Convert to little-endian format
+
+            // T5 BMS_Main_Relay_Cmd == 1 && VCU_MotorMode = 1/2
+            vcu1.BMS_Main_Relay_Cmd = 1; // 1 = ON, 0 = OFF
+            bms1.Pre_charge_Relay_FB =
+                1; // 1 = ON, 0 = OFF NOTE: see if we can omit this bit
+            bms1.Pre_charge_Finish_Sts = 1; // 1 = ON, 0 = OFF
+
+            vcu1.VehicleState = 1; // 0 = Not ready, 1 = Ready
+            vcu1.GearLeverPos_Sts =
+                3;                   // 0 = Default, 1 = R, 2 = N, 3 = D, 4 = P
+            vcu1.AC_Control_Cmd = 1; // 0 = Not active, 1 = Active
+            vcu1.BMS_Aux_Relay_Cmd = 1; // 0 = not work, 1 = work
+            vcu1.VCU_WorkMode = 0;
+
+            // vcu1.KeyPosition = 2; // 0 = Off, 1 = ACC, 2 = ON, 3 = Crank+On
+
+            vcu1.VCU_TorqueReq =
+                (uint8_t)((fabsf(motorData.desiredTorque) / MOTOR_MAX_TORQUE) *
+                          100); // Torque demand in percentage (0-99.6) 350Nm
+            vcu1.VCU_MotorMode = motorData.desiredTorque >= 0
+                                     ? 1
+                                     : 2; // 0 = Standby, 1 = Drive, 2 =
+                                          // Generate Electricy, 3 = Reserved
+            break;
+        }
+
+        case MOTOR_STATE_FAULT: {
+            // T7 MCU_Warning_Level == 3
+            vcu1.BMS_Main_Relay_Cmd = 0; // 1     = ON, 0 = OFF
+            bms1.Pre_charge_Relay_FB =
+                0; // 1     = ON, 0 = OFF NOTE: see if we can omit this bit
+            bms1.Pre_charge_Finish_Sts = 0; // 1   = ON, 0 = OFF
+            // vcu1.VCU_Warning_Level = 3; // 1    0  = No Warning, 1 = Warning,
+            // 2 = Fault, 3 = Critical Fault
+            vcu1.VCU_MotorMode = 0; // 1       0 = Standby, 1 = Drive, 2 =
+                                    // Generate// Electricy, 3 = Reserved
+            break;
+        }
+
+        default: {
+            break;
+        }
         }
 
-        vcu1.CheckSum = ComputeChecksum((uint8_t*) &vcu1);
-        bms1.CheckSum = ComputeChecksum((uint8_t*) &bms1);
-        bms2.CheckSum = ComputeChecksum((uint8_t*) &bms2);
+        vcu1.CheckSum = ComputeChecksum((uint8_t *)&vcu1);
+        bms1.CheckSum = ComputeChecksum((uint8_t *)&bms1);
+        bms2.CheckSum = ComputeChecksum((uint8_t *)&bms2);
 
         uint64_t vcu1_msg;
         memcpy(&vcu1_msg, &vcu1, sizeof(vcu1_msg));
@@ -118,99 +186,128 @@ void threadMotor(void *pvParameters){
     }
 }
 
-void Motor_UpdateMotor(float torqueDemand, bool enablePrecharge, bool enablePower, bool enableRun, bool enableRegen){
+void Motor_UpdateMotor(float torqueDemand, bool enablePrecharge,
+                       bool enablePower, bool enableRun, bool enableRegen,
+                       bool enableStandby) {
     // Update the motor state based on the RTM button state
+    // float throttleCommand = APPS_GetAPPSReading(); // 0; //TODO Get
+    // APPS_travel
 
-    // float throttleCommand = APPS_GetAPPSReading(); // 0; //TODO Get APPS_travel
-    switch(motorData.state){
-        // LV on, HV off
-        case MOTOR_STATE_OFF:{
-            if (enablePrecharge){
-                # if DEBUG_FLAG
-                    Serial.println("Precharging...");
-                #endif
-                motorData.state = MOTOR_STATE_PRECHARGING;
-            }
-            motorData.desiredTorque = 0.0F;
-            break;
+    RTMButton_Update(GPIO_Read(RTM_BUTTON_PIN));
+
+    uint8_t prechargeState = PCC_GetData()->state;
+    uint16_t prechargeProg = PCC_GetData()->prechargeProgress;
+    // off --> standby --> precharge --> run --> fault -->standy
+    // no kl15 then off
+    switch (motorData.state) {
+    // LV on, HV off
+
+    // no kl15
+    case MOTOR_STATE_OFF: {
+        if (enableStandby) {
+            // # if HIMAC_FLAG
+            //     Serial.println("Standby Mode");
+            // #endif
+            motorData.state = MOTOR_STATE_STANDBY;
         }
-        // HV switch on (PCC CAN message)
-        case MOTOR_STATE_PRECHARGING:
-        {
-            if(enablePower){
-                # if DEBUG_FLAG
-                    Serial.println("Precharge finished");
-                # endif
-                motorData.state = MOTOR_STATE_IDLE;
-            }
-            motorData.desiredTorque = 0.0F;
-            break;
+        motorData.desiredTorque = 0.0F;
+        break;
+    }
+    // kl15 on and ready to go into precharge
+    case MOTOR_STATE_STANDBY: {
+        if (enablePrecharge) {
+
+            motorData.state = MOTOR_STATE_PRECHARGING;
         }
-        // PCC CAN message finished
-        case MOTOR_STATE_IDLE:
-        {
-            if(enableRun){
-                # if DEBUG_FLAG
-                    Serial.println("Ready to drive...");
-                # endif
-                motorData.state = MOTOR_STATE_DRIVING;
-            }
-            motorData.desiredTorque = 0.0F;
-            break;
+        motorData.desiredTorque = 0.0F;
+        break;
+    }
+    // HV switch on (PCC CAN message)
+    case MOTOR_STATE_PRECHARGING: {
+        if (prechargeProg >= 94 && prechargeState == 3) {
+            // # if HIMAC_FLAG
+            //     Serial.println("Precharge finished");
+            // # endif
+            motorData.state = MOTOR_STATE_IDLE;
+        } else if (enableStandby) {
+            motorData.state = MOTOR_STATE_STANDBY;
+        }
+
+        motorData.desiredTorque = 0.0F;
+        break;
+    }
+    // PCC CAN message finished
+    case MOTOR_STATE_IDLE: {
+        if (RTMButton_GetState()) {
+#if HIMAC_FLAG
+            // Serial.println("Ready to drive...");
+#endif
+            motorData.state = MOTOR_STATE_DRIVING;
         }
-        // Ready to drive button pressed
-        case MOTOR_STATE_DRIVING:
-        {
-            // if(!enableRun){
-            //     motorData.state = MOTOR_STATE_IDLE;
-            // }
-
-            // torque is communicated as a percentage
-            #if !SPEED_CONTROL_ENABLED
-
-            if (enableRegen && torqueDemand <= 0.0F && MCU_GetMCU1Data()->motorDirection == MOTOR_DIRECTION_FORWARD) {
-                // If regen is enabled and the torque demand is zero, we need to set the torque demand to 0
-                // to prevent the motor from applying torque in the wrong direction
+        motorData.desiredTorque = 0.0F;
+        break;
+    }
+    // Ready to drive button pressed
+    case MOTOR_STATE_DRIVING: {
+// if(!enableRun){
+//     motorData.state = MOTOR_STATE_IDLE;
+// }
+
+// if (enablePower) {
+//     motorData.state = MOTOR_STATE_IDLE;
+// }
+// torque is communicated as a percentage
+#if !SPEED_CONTROL_ENABLED
+        if (RTMButton_GetState()) {
+
+            if (enableRegen && torqueDemand <= 0.0F &&
+                MCU_GetMCU1Data()->motorDirection == MOTOR_DIRECTION_FORWARD) {
+                // If regen is enabled and the torque demand is zero, we need to
+                // set the torque demand to 0 to prevent the motor from applying
+                // torque in the wrong direction
                 motorData.desiredTorque = MAX_REGEN_TORQUE * REGEN_BIAS;
             } else {
                 motorData.desiredTorque = torqueDemand;
             }
-            #else
-            // Speed control is enabled, we need to set the torque demand to 0
-            vcu1.VCU_TorqueReq = 0; // 0 = No torque
-            #endif
 
-            break;
-        }
-        case MOTOR_STATE_FAULT:
-        {
-            // TODO Implement RTM Button
-            if(RTMButton_GetState() == false){
-                motorData.state = MOTOR_STATE_IDLE;
-            }
-            motorData.desiredTorque = 0.0F;
-            break;
-        }
-        default:
-        {
-            break;
+        } else {
+            motorData.state = MOTOR_STATE_IDLE;
+            torqueDemand = 0;
         }
+
+#else
+        // Speed control is enabled, we need to set the torque demand to 0
+        vcu1.VCU_TorqueReq = 0; // 0 = No torque
+#endif
+
+        break;
     }
-}
+    // Any fault error occurs
+    case MOTOR_STATE_FAULT: {
 
-float Motor_GetTorqueDemand(){
-    return motorData.desiredTorque;
+        if (enableStandby) {
+            motorData.state = MOTOR_STATE_STANDBY;
+        } else if (enableRun) {
+            Motor_ClearFaultState();
+        }
+        // GOES BACK TO HV ON
+        //  TODO Implement RTM Button
+        //  if(RTMButton_GetState() == false){
+        //      motorData.state = MOTOR_STATE_IDLE;
+        //  }
+        motorData.desiredTorque = 0.0F;
+        break;
+    }
+    default: {
+        break;
+    }
+    }
 }
 
-void Motor_SetFaultState(){
-    motorData.state = MOTOR_STATE_FAULT;
-}
+float Motor_GetTorqueDemand() { return motorData.desiredTorque; }
 
-void Motor_ClearFaultState(){
-    motorData.state = MOTOR_STATE_DRIVING;
-}
+void Motor_SetFaultState() { motorData.state = MOTOR_STATE_FAULT; }
 
-MotorState Motor_GetState(){
-    return motorData.state;
-}
+void Motor_ClearFaultState() { motorData.state = MOTOR_STATE_DRIVING; }
 
+MotorState Motor_GetState() { return motorData.state; }
diff --git a/fsae-vehicle-fw/src/vehicle/motor.h b/fsae-vehicle-fw/src/vehicle/motor.h
index 78108d1..c94a0ef 100644
--- a/fsae-vehicle-fw/src/vehicle/motor.h
+++ b/fsae-vehicle-fw/src/vehicle/motor.h
@@ -6,16 +6,19 @@
 
 typedef enum {
     MOTOR_STATE_OFF,
+    MOTOR_STATE_STANDBY,
     MOTOR_STATE_PRECHARGING,
     MOTOR_STATE_IDLE,
     MOTOR_STATE_DRIVING,
-    MOTOR_STATE_FAULT,
+    MOTOR_STATE_FAULT
 } MotorState;
 
 void threadMotor(void *pvParameters);
 
 void Motor_Init();
-void Motor_UpdateMotor(float torqueDemand, bool enablePrecharge, bool enablePower, bool enableRun, bool enableRegen);
+void Motor_UpdateMotor(float torqueDemand, bool enablePrecharge,
+                       bool enablePower, bool enableRun, bool enableRegen,
+                       bool enableStandby);
 
 float Motor_GetTorqueDemand();
 MotorState Motor_GetState();
diff --git a/fsae-vehicle-fw/src/vehicle/pcc_receive.cpp b/fsae-vehicle-fw/src/vehicle/pcc_receive.cpp
new file mode 100644
index 0000000..289821f
--- /dev/null
+++ b/fsae-vehicle-fw/src/vehicle/pcc_receive.cpp
@@ -0,0 +1,26 @@
+#define THREAD_CAN_STACK_SIZE 128
+#define THREAD_CAN_PRIORITY 1
+
+#include "pcc_receive.h"
+#include "arduino_freertos.h"
+
+static PCC pccData;
+
+void PCC_Init() {
+    pccData = {.state = 0,
+               .errorCode = 0,
+               .accumulatorVoltage = 0,
+               .tsVoltage = 0,
+               .prechargeProgress = 404};
+}
+
+void processPCCMessage(uint64_t rx_data) {
+    taskENTER_CRITICAL();
+    memcpy(&pccData, &rx_data, sizeof(PCC));
+    taskEXIT_CRITICAL();
+
+    // Serial.print("Precharge Progress: ");
+    // Serial.println(pccData.prechargeProgress);
+}
+
+PCC *PCC_GetData() { return &pccData; }
diff --git a/fsae-vehicle-fw/src/vehicle/pcc_receive.h b/fsae-vehicle-fw/src/vehicle/pcc_receive.h
new file mode 100644
index 0000000..1e2dd97
--- /dev/null
+++ b/fsae-vehicle-fw/src/vehicle/pcc_receive.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdint.h>
+
+typedef struct __attribute__((packed)) {
+    // uint32_t timestamp;
+    uint8_t state;
+    uint8_t errorCode;
+    uint16_t accumulatorVoltage;
+    uint16_t tsVoltage;
+    uint16_t prechargeProgress;
+} PCC;
+
+// static PCC pccData;
+
+void PCC_Init();
+void processPCCMessage(uint64_t);
+
+PCC *PCC_GetData();
diff --git a/fsae-vehicle-fw/src/vehicle/rtm_button.cpp b/fsae-vehicle-fw/src/vehicle/rtm_button.cpp
index be0a938..3784a50 100644
--- a/fsae-vehicle-fw/src/vehicle/rtm_button.cpp
+++ b/fsae-vehicle-fw/src/vehicle/rtm_button.cpp
@@ -1,22 +1,21 @@
 // Anteater Electric Racing, 2025
 
-#define BUTTON_DEBOUNCE_MS 100
+#define BUTTON_DEBOUNCE_MS 500
 
 #include <Arduino.h>
 #include <stdint.h>
 
 #include "rtm_button.h"
 
-static bool rtmState = false; // Latching state of RTM based on momentary button press. True - driving state, false - idle state
+static bool rtmState = false; // Latching state of RTM based on momentary button
+                              // press. True - driving state, false - idle state
 static uint32_t lastDebounceTime = 0;
 
 void RTMButton_Update(bool rtmButton) {
-    if(rtmButton == 1 && millis() - lastDebounceTime > BUTTON_DEBOUNCE_MS) {
+    if (rtmButton == 1 && millis() - lastDebounceTime > BUTTON_DEBOUNCE_MS) {
         rtmState = !rtmState; // Toggle the state
         lastDebounceTime = millis();
     }
 }
 
-bool RTMButton_GetState() {
-    return rtmState;
-}
+bool RTMButton_GetState() { return rtmState; }
diff --git a/fsae-vehicle-fw/src/vehicle/telemetry.cpp b/fsae-vehicle-fw/src/vehicle/telemetry.cpp
index 1ab42c5..171e871 100644
--- a/fsae-vehicle-fw/src/vehicle/telemetry.cpp
+++ b/fsae-vehicle-fw/src/vehicle/telemetry.cpp
@@ -15,101 +15,78 @@
 TelemetryData telemetryData;
 
 void Telemetry_Init() {
-    // TODO: Update initialization
-    telemetryData = { // Fill with reasonable dummy values
+    telemetryData = {
+        // Fill with reasonable dummy values
         .APPS_Travel = 0.0F,
-        .BSEFront_PSI = 0.0F,
-        .BSERear_PSI = 0.0F,
-        .accumulatorVoltage = 0.0F,
-        .accumulatorTemp_F = 25.0F,
-        .motorState = MOTOR_STATE_OFF,
         .motorSpeed = 0.0F,
         .motorTorque = 0.0F,
         .maxMotorTorque = 0.0F,
-        .maxMotorBrakeTorque = 0.0F,
         .motorDirection = DIRECTION_STANDBY,
+        .motorState = MOTOR_STATE_OFF,
+
         .mcuMainState = STATE_STANDBY,
         .mcuWorkMode = WORK_MODE_STANDBY,
+
+        .mcuVoltage = 0.0F,
+        .mcuCurrent = 0.0F,
         .motorTemp = 25,
         .mcuTemp = 25,
+
         .dcMainWireOverVoltFault = false,
-        .motorPhaseCurrFault = false,
-        .mcuOverHotFault = false,
-        .resolverFault = false,
-        .phaseCurrSensorFault = false,
-        .motorOverSpdFault = false,
-        .drvMotorOverHotFault = false,
         .dcMainWireOverCurrFault = false,
-        .drvMotorOverCoolFault = false,
-        .mcuMotorSystemState = false,
-        .mcuTempSensorState = false,
-        .motorTempSensorState = false,
-        .dcVoltSensorState = false,
-        .dcLowVoltWarning = false,
-        .mcu12VLowVoltWarning = false,
+        .motorOverSpdFault = false,
+        .motorPhaseCurrFault = false,
         .motorStallFault = false,
-        .motorOpenPhaseFault = false,
+
         .mcuWarningLevel = ERROR_NONE,
-        .mcuVoltage = 0.0F,
-        .mcuCurrent = 0.0F,
-        .motorPhaseCurr = 0.0F,
+
     };
 }
 
-void threadTelemetry(void *pvParameters){
-    static TickType_t lastWakeTime = xTaskGetTickCount(); // Initialize the last wake time
-    while(true){
+void threadTelemetry(void *pvParameters) {
+    static TickType_t lastWakeTime =
+        xTaskGetTickCount(); // Initialize the last wake time
+    while (true) {
         taskENTER_CRITICAL(); // Enter critical section
         telemetryData = {
             .APPS_Travel = APPS_GetAPPSReading(),
-            .BSEFront_PSI = BSE_GetBSEReading()->bseFront_PSI,
-            .BSERear_PSI = BSE_GetBSEReading()->bseFront_PSI,
-            .accumulatorVoltage = 0.0F, // TODO: Replace with actual accumulator voltage reading
-            .accumulatorTemp_F = 0.0F, // TODO: Replace with actual accumulator temperature reading
-            .motorState = Motor_GetState(),
 
             .motorSpeed = MCU_GetMCU1Data()->motorSpeed,
             .motorTorque = MCU_GetMCU1Data()->motorTorque,
             .maxMotorTorque = MCU_GetMCU1Data()->maxMotorTorque,
-            .maxMotorBrakeTorque = MCU_GetMCU1Data()->maxMotorBrakeTorque,
-            .motorDirection = MCU_GetMCU1Data()->motorDirection,
+            // .maxMotorBrakeTorque = MCU_GetMCU1Data()->maxMotorBrakeTorque,
+
+            // .motorDirection = MCU_GetMCU1Data()->motorDirection,
+            .motorState = Motor_GetState(),
             .mcuMainState = MCU_GetMCU1Data()->mcuMainState,
             .mcuWorkMode = MCU_GetMCU1Data()->mcuWorkMode,
 
+            .mcuVoltage = MCU_GetMCU3Data()->mcuVoltage,
+            .mcuCurrent = MCU_GetMCU3Data()->mcuCurrent,
             .motorTemp = MCU_GetMCU2Data()->motorTemp,
             .mcuTemp = MCU_GetMCU2Data()->mcuTemp,
-            .dcMainWireOverVoltFault = MCU_GetMCU2Data()->dcMainWireOverVoltFault,
-            .motorPhaseCurrFault = MCU_GetMCU2Data()->motorPhaseCurrFault,
-            .mcuOverHotFault = MCU_GetMCU2Data()->mcuOverHotFault,
-            .resolverFault = MCU_GetMCU2Data()->resolverFault,
-            .phaseCurrSensorFault = MCU_GetMCU2Data()->phaseCurrSensorFault,
+
+            .dcMainWireOverVoltFault =
+                MCU_GetMCU2Data()->dcMainWireOverVoltFault,
+            .dcMainWireOverCurrFault =
+                MCU_GetMCU2Data()->dcMainWireOverCurrFault,
             .motorOverSpdFault = MCU_GetMCU2Data()->motorOverSpdFault,
-            .drvMotorOverHotFault = MCU_GetMCU2Data()->drvMotorOverHotFault,
-            .dcMainWireOverCurrFault = MCU_GetMCU2Data()->dcMainWireOverCurrFault,
-            .drvMotorOverCoolFault = MCU_GetMCU2Data()->drvMotorOverCoolFault,
-            .mcuMotorSystemState = MCU_GetMCU2Data()->mcuMotorSystemState,
-            .mcuTempSensorState = MCU_GetMCU2Data()->mcuTempSensorState,
-            .motorTempSensorState = MCU_GetMCU2Data()->motorTempSensorState,
-            .dcVoltSensorState = MCU_GetMCU2Data()->dcVoltSensorState,
-            .dcLowVoltWarning = MCU_GetMCU2Data()->dcLowVoltWarning,
-            .mcu12VLowVoltWarning = MCU_GetMCU2Data()->mcu12VLowVoltWarning,
+            .motorPhaseCurrFault = MCU_GetMCU2Data()->motorPhaseCurrFault,
+
             .motorStallFault = MCU_GetMCU2Data()->motorStallFault,
-            .motorOpenPhaseFault = MCU_GetMCU2Data()->motorOpenPhaseFault,
             .mcuWarningLevel = MCU_GetMCU2Data()->mcuWarningLevel,
 
-            .mcuVoltage = MCU_GetMCU3Data()->mcuVoltage,
-            .mcuCurrent = MCU_GetMCU3Data()->mcuCurrent,
-            .motorPhaseCurr = MCU_GetMCU3Data()->motorPhaseCurr,
         };
         taskEXIT_CRITICAL();
 
-        uint8_t* serializedData = (uint8_t*) &telemetryData;
+        uint8_t *serializedData = (uint8_t *)&telemetryData;
         CAN_ISOTP_Send(TELEMETRY_CAN_ID, serializedData, sizeof(TelemetryData));
 
-        vTaskDelayUntil(&lastWakeTime, pdMS_TO_TICKS(TELEMETRY_PERIOD_MS)); // Delay until the next telemetry update
+        vTaskDelayUntil(
+            &lastWakeTime,
+            pdMS_TO_TICKS(
+                TELEMETRY_PERIOD_MS)); // Delay until the next telemetry update
     }
 }
 
-TelemetryData const* Telemetry_GetData() {
-    return &telemetryData;
-}
+TelemetryData const *Telemetry_GetData() { return &telemetryData; }
diff --git a/fsae-vehicle-fw/src/vehicle/telemetry.h b/fsae-vehicle-fw/src/vehicle/telemetry.h
index b1c595e..9617d15 100644
--- a/fsae-vehicle-fw/src/vehicle/telemetry.h
+++ b/fsae-vehicle-fw/src/vehicle/telemetry.h
@@ -3,60 +3,70 @@
 
 #include <stdint.h>
 
-#include "vehicle/motor.h"
 #include "peripherals/adc.h"
 #include "vehicle/ifl100-36.h"
+#include "vehicle/motor.h"
 
-typedef struct __attribute__((packed)){
+typedef struct __attribute__((packed)) {
     float APPS_Travel; // APPS travel in %
 
-    float BSEFront_PSI; // front brake pressure in PSI
-    float BSERear_PSI; // rear brake pressure in PSI
+    // float BSEFront_PSI; // front brake pressure in PSI
+    // float BSERear_PSI; // rear brake pressure in PSI
 
-    float accumulatorVoltage;
-    float accumulatorTemp_F;
+    // float accumulatorVoltage;
+    // float accumulatorTemp_F;
 
-    MotorState motorState; // Motor state
+    // Motor state
 
     // MCU1 data
-    float motorSpeed; // Motor speed in RPM
-    float motorTorque; // Motor torque in Nm
+    float motorSpeed;     // Motor speed in RPM
+    float motorTorque;    // Motor torque in Nm
     float maxMotorTorque; // Max motor torque in Nm
-    float maxMotorBrakeTorque; // Max motor brake torque in Nm
+    // float maxMotorBrakeTorque; // Max motor brake torque in Nm
     MotorRotateDirection motorDirection; // Motor direction
+    MotorState motorState;
+
     MCUMainState mcuMainState; // Motor main state
-    MCUWorkMode mcuWorkMode; // MCU work mode
+    MCUWorkMode mcuWorkMode;   // MCU work mode
+
+    float mcuVoltage;
+    float mcuCurrent;
 
     // MCU2 data
     int32_t motorTemp; // Motor temperature in C
-    int32_t mcuTemp; // Inverter temperature in C
+    int32_t mcuTemp;   // Inverter temperature in C
+
     bool dcMainWireOverVoltFault; // DC over voltage fault
-    bool motorPhaseCurrFault; // MCU motor phase current fault
-    bool mcuOverHotFault; // MCU overheat fault
-    bool resolverFault; // Resolver fault
-    bool phaseCurrSensorFault; // Phase current sensor fault
-    bool motorOverSpdFault; // MCU motor over speed fault
-    bool drvMotorOverHotFault; // Driver motor overheat fault
     bool dcMainWireOverCurrFault; // DC main wire over voltage fault
-    bool drvMotorOverCoolFault; // Driver motor overcool fault
-    bool mcuMotorSystemState; // MCU motor system state
-    bool mcuTempSensorState; // MCU temperature sensor state
-    bool motorTempSensorState; // MCU motor temperature sensor state
-    bool dcVoltSensorState; // MCU DC voltage sensor state
-    bool dcLowVoltWarning; // MCU DC low voltage warning
-    bool mcu12VLowVoltWarning; // MCU 12V low voltage warning
+    bool motorOverSpdFault;       // MCU motor over speed fault
+    //  bool phaseCurrSensorFault; // Phase current sensor fault
+
+    bool motorPhaseCurrFault; // MCU motor phase current fault
+    // bool mcuOverHotFault; // MCU overheat fault
+    // bool resolverFault; // Resolver fault
+    //    bool phaseCurrSensorFault; // Phase current sensor fault
+    //    bool motorOverSpdFault; // MCU motor over speed fault
+    //     bool drvMotorOverHotFault; // Driver motor overheat fault
+    //  //   bool dcMainWireOverCurrFault; // DC main wire over voltage fault
+    //     bool drvMotorOverCoolFault; // Driver motor overcool fault
+    //     bool mcuMotorSystemState; // MCU motor system state
+    //     bool mcuTempSensorState; // MCU temperature sensor state
+    //     bool motorTempSensorState; // MCU motor temperature sensor state
+    //     bool dcVoltSensorState; // MCU DC voltage sensor state
+    //     bool dcLowVoltWarning; // MCU DC low voltage warning
+    //     bool mcu12VLowVoltWarning; // MCU 12V low voltage warning
     bool motorStallFault; // MCU motor stall fault
-    bool motorOpenPhaseFault; // MCU motor open phase fault
+    //    bool motorOpenPhaseFault; // MCU motor open phase fault
     MCUWarningLevel mcuWarningLevel; // MCU warning level
 
     // MCU3 data
-    float mcuVoltage; // DC main wire voltage in V
-    float mcuCurrent; // DC main wire current in A
-    float motorPhaseCurr; // Motor phase current in A
+    // float mcuVoltage; // DC main wire voltage in V
+    // float mcuCurrent; // DC main wire current in A
+    //   float motorPhaseCurr; // Motor phase current in A
 
     float debug[4]; // Debug data
 } TelemetryData;
 
 void Telemetry_Init();
 void threadTelemetry(void *pvParameters);
-TelemetryData const* Telemetry_GetData();
+TelemetryData const *Telemetry_GetData();
diff --git a/fsae-vehicle-fw/src/vehicle/thermal.cpp b/fsae-vehicle-fw/src/vehicle/thermal.cpp
new file mode 100644
index 0000000..1c78873
--- /dev/null
+++ b/fsae-vehicle-fw/src/vehicle/thermal.cpp
@@ -0,0 +1,53 @@
+#include "vehicle/telemetry.h"
+#include <Arduino.h>
+
+#define DUTY_CYCLE_MAX 255
+#define ANALOG_WRITE_FREQUENCY 25000 // 25 kHz for Koolance
+#define ANALOG_WRITE_RESOLUTION 8    // 8-bit resolution (0-255)
+
+#define PUMP_PIN 12 // Define the PWM pin for the pump
+#define FAN_PIN 7
+
+#define TEMP_THRESHOLD 30 // Temperature threshold in degrees Celsius
+
+void thermal_Init() {
+    pinMode(PUMP_PIN, OUTPUT);
+    analogWriteFrequency(PUMP_PIN,
+                         ANALOG_WRITE_FREQUENCY);   // 25 kHz for Koolance
+    analogWriteResolution(ANALOG_WRITE_RESOLUTION); // 0-255
+
+    pinMode(FAN_PIN, OUTPUT);
+    analogWriteFrequency(FAN_PIN,
+                         ANALOG_WRITE_FREQUENCY);   // 25 kHz for Koolance
+    analogWriteResolution(ANALOG_WRITE_RESOLUTION); // 0-255
+}
+
+void thermal_Update(uint32_t rawReading1, uint32_t rawReading2,
+                    uint32_t rawReading3, uint32_t rawReading4) {
+    // Assuming rawReading1 and rawReading2 are the temperature readings from
+    // the sensors Convert raw readings to temperature in degrees Celsius
+    int32_t temp1 = (rawReading1);
+    int32_t temp2 = (rawReading2);
+    // int32_t temp3 = (rawReading3);
+    // int32_t temp4 = (rawReading4);
+    // Check if the MCU temperature exceeds the threshold
+    if (temp1 > TEMP_THRESHOLD || temp2 > TEMP_THRESHOLD) {
+        // If the MCU temperature exceeds the threshold, turn on the pump and
+        // fan
+        analogWrite(PUMP_PIN,
+                    DUTY_CYCLE_MAX * 0.9); // Set pump to full duty cycle
+        analogWrite(FAN_PIN,
+                    DUTY_CYCLE_MAX * 0.9); // Set fan to full duty cycle
+    } else {
+        // If the MCU temperature is below the threshold, turn off the pump and
+        // fan
+        analogWrite(PUMP_PIN, 0); // Set pump to 0 duty cycle
+        analogWrite(FAN_PIN, 0);  // Set fan to 0 duty cycle
+    }
+    // Debugging output
+    // Serial.print("Temp1: ");
+    // Serial.print(temp1);
+    // Serial.print(" C | Temp2: ");
+    // Serial.print(temp2);
+    // Serial.println(" C");
+}
diff --git a/fsae-vehicle-fw/src/vehicle/thermal.h b/fsae-vehicle-fw/src/vehicle/thermal.h
new file mode 100644
index 0000000..88d95be
--- /dev/null
+++ b/fsae-vehicle-fw/src/vehicle/thermal.h
@@ -0,0 +1,6 @@
+
+#include <stdint.h>
+
+void thermal_Init();
+void thermal_Update(uint32_t rawReading1, uint32_t rawReading2,
+                    uint32_t rawReading3, uint32_t rawReading4);