diff --git a/firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/generic_mcu.cpp b/firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/generic_mcu.cpp
index dec7205..9172825 100644
--- a/firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/generic_mcu.cpp
+++ b/firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/generic_mcu.cpp
@@ -1,4 +1,6 @@
 #include "../hardware_api.h"
+#include "communication/SimpleFOCDebug.h"
+
 
 // function reading an ADC value and returning the read voltage
 __attribute__((weak))  float _readADCVoltageInline(const int pinA, const void* cs_params){
diff --git a/firmware/lib/currentsense/InlineCurrentSenseSync.cpp b/firmware/lib/currentsense/InlineCurrentSenseSync.cpp
index bffa346..b835ee2 100644
--- a/firmware/lib/currentsense/InlineCurrentSenseSync.cpp
+++ b/firmware/lib/currentsense/InlineCurrentSenseSync.cpp
@@ -37,6 +37,7 @@ InlineCurrentSenseSync::InlineCurrentSenseSync(float _mVpA, int _pinA, int _pinB
 
 // Inline sensor init function
 int InlineCurrentSenseSync::init(){
+    SIMPLEFOC_DEBUG("CS:Init Current Sense Inline Sync");
     // if no linked driver its fine in this case 
     // at least for init()
     void* drv_params = driver ? driver->params : nullptr;
diff --git a/firmware/lib/currentsense/utils.cpp b/firmware/lib/currentsense/utils.cpp
index 244e466..0142b5f 100644
--- a/firmware/lib/currentsense/utils.cpp
+++ b/firmware/lib/currentsense/utils.cpp
@@ -24,8 +24,9 @@ void MX_GPIO_Init(void)
     __HAL_RCC_ADC12_CLK_ENABLE();
 }
 
-float _readVoltageInline(const uint8_t pin, const void *cs_params)
+float _readADCVoltageInline(const int pin, const void *cs_params)
 {
+    // SIMPLEFOC_DEBUG("READ ADC VOLTAGE");
     uint32_t rawResult;
     switch (pin)
     {
@@ -60,6 +61,7 @@ float _readVoltageLowSide(const int pinA, const void *cs_params)
 
 void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
 {
+    SIMPLEFOC_DEBUG("Configure Utils ADCInline");
     _UNUSED(driver_params);
 
     HAL_Init();
@@ -106,6 +108,7 @@ void *_configureADCLowSide(const void *driver_params, const int pinA, const int
 
 void _driverSyncLowSide(void *_driver_params, void *_cs_params)
 {
+    SIMPLEFOC_DEBUG("CS: Sync CS to driver");
     STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
     Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
 
diff --git a/firmware/lib/currentsense/utils.h b/firmware/lib/currentsense/utils.h
index bcbded4..18a5957 100644
--- a/firmware/lib/currentsense/utils.h
+++ b/firmware/lib/currentsense/utils.h
@@ -6,9 +6,16 @@
 #include "stm32g4xx_hal.h"
 
 #include "communication/SimpleFOCDebug.h"
-#include "current_sense/hardware_api.h"
+// #include "current_sense/hardware_api.h"
 #include "current_sense/hardware_specific/stm32/stm32_mcu.h"
 #include "drivers/hardware_specific/stm32/stm32_mcu.h"
+float _readADCVoltageInline(const int pinA, const void* cs_params);
+// float _readADCVoltageInline(const uint8_t pin, const void *cs_params);
+float _readVoltageLowSide(const int pinA, const void *cs_params);
+void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC);
+void *_configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC);
+void _driverSyncLowSide(void *_driver_params, void *_cs_params);
+
 
 #endif
 #endif
\ No newline at end of file
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index 908e528..c7a10fa 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -3,8 +3,8 @@
 #include <SPI.h>
 
 #include <SimpleFOC.h>
-#include <SimpleFOCDrivers.h>
-#include "encoders/MT6835/MagneticSensorMT6835.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/mt6835/MagneticSensorMT6835.h"
 #include "encoders/stm32hwencoder/STM32HWEncoder.h"
 
 #include "stm32g4xx_hal_conf.h"
@@ -45,7 +45,9 @@ extern uint8_t RxData[8];
 #define MOTORKV 40
 #define ENC_PPR 16383 // max 16383 (zero index) -> *4 for CPR, -1 is done in init to prevent rollover on 16 bit timer
 
-#define SERIALPORT SerialUSB
+#define SERIALPORT Serial3
+
+HardwareSerial Serial3 = HardwareSerial(PB8, PB9);
 
 /**
  * SPI clockdiv of 16 gives ~10.5MHz clock. May still be stable with lower divisor.
@@ -86,11 +88,12 @@ void setup()
 	digitalWrite(MOT_EN, HIGH);
 	digitalWrite(CAL_EN, LOW);
 
-	uint8_t ret = configureCAN();
-	if (!ret){
-		SIMPLEFOC_DEBUG("CAN init failed.");
-		digitalWrite(LED_FAULT, HIGH);
-	}
+	uint8_t ret;
+	// ret = configureCAN();
+	// if (!ret){
+	// 	SIMPLEFOC_DEBUG("CAN init failed.");
+	// 	digitalWrite(LED_FAULT, HIGH);
+	// }
 	ret = configureFOC();
 	if (!ret){
 		SIMPLEFOC_DEBUG("FOC init failed.");
@@ -254,8 +257,8 @@ uint8_t configureFOC(void)
 	motor.linkDriver(&driver);
 
 	currentsense.linkDriver(&driver);
-	currentsense.init();
-
+	int ret = currentsense.init();
+	SERIALPORT.printf("Current Sense init result: %i\n", ret);
 	motor.linkCurrentSense(&currentsense);
 
 	motor.target = 10;