diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp
index 7a74447..e548c2d 100644
--- a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp
+++ b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp
@@ -110,12 +110,12 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el)
 /**
     Driver linking to the current sense
 */
-void CurrentSense::linkDriver(BLDCDriver *_driver)
+void CurrentSense::linkDriver(FOCDriver *_driver)
 {
     driver = _driver;
 }
 
-void CurrentSense::linkDriver(StepperDriver *_driver)
-{
-    driver = _driver;
-}
\ No newline at end of file
+// void CurrentSense::linkDriver(StepperDriver *_driver)
+// {
+//     driver = _driver;
+// }
diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h
index 00fa0cf..2672dff 100644
--- a/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h
+++ b/firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h
@@ -24,8 +24,8 @@ class CurrentSense{
      * Linking the current sense with the motor driver
      * Only necessary if synchronisation in between the two is required
      */
-    void linkDriver(BLDCDriver *driver);
-    void linkDriver(StepperDriver *driver);
+    void linkDriver(FOCDriver *driver);
+    // void linkDriver(StepperDriver *driver);
 
     // variables
     bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
@@ -72,4 +72,4 @@ class CurrentSense{
     DQCurrent_s getFOCCurrents(float angle_el);
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h b/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h
index 67343f7..7a60c4b 100644
--- a/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h
+++ b/firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h
@@ -1,7 +1,8 @@
-#ifndef FOCMOTOR_H
-#define FOTMOTOR_H
+#ifndef FOCDRIVER_H
+#define FOCDRIVER_H
 
 #include "Arduino.h"
+#include "../foc_utils.h"
 
 class FOCDriver{
     public: 
@@ -11,6 +12,9 @@ class FOCDriver{
 
         virtual void disable() = 0;
 
+        virtual void setPwm(float a, float b, float c) = 0;
+        // virtual void setPwm(uint8_t a, uint8_t b);
+
         long pwm_frequency;
         float voltage_power_supply;
         float voltage_limit;
@@ -18,4 +22,4 @@ class FOCDriver{
         void* params = 0;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h b/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h
index f0369ec..1793164 100644
--- a/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h
+++ b/firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h
@@ -12,7 +12,7 @@ class StepperDriver: public FOCDriver{
          * @param Ua phase A voltage
          * @param Ub phase B voltage
         */
-        virtual void setPwm(float Ua, float Ub) = 0;
+        virtual void setPwm(float Ua, float Ub, float Uc=NOT_SET) = 0;
 };
 
-#endif
\ No newline at end of file
+#endif
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/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp
index dbbf5b8..f690d42 100644
--- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp
+++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp
@@ -86,7 +86,7 @@ int StepperDriver2PWM::init() {
 
 
 // Set voltage to the pwm pin
-void StepperDriver2PWM::setPwm(float Ua, float Ub) {
+void StepperDriver2PWM::setPwm(float Ua, float Ub, float Uc) {
   float duty_cycle1(0.0f),duty_cycle2(0.0f);
   // limit the voltage in driver
   Ua = _constrain(Ua, -voltage_limit, voltage_limit);
@@ -104,4 +104,4 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) {
 
   // write to hardware
   _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params);
-}
\ No newline at end of file
+}
diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h
index b349af0..6b6f978 100644
--- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h
+++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h
@@ -58,7 +58,7 @@ class StepperDriver2PWM: public StepperDriver
      * @param Ua phase A voltage
      * @param Ub phase B voltage
     */
-    void setPwm(float Ua, float Ub) override;
+    void setPwm(float Ua, float Ub, float NOT_USED=NOT_SET) override;
 
   private:
         
diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp
index 836f547..9777855 100644
--- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp
+++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp
@@ -60,8 +60,9 @@ int StepperDriver4PWM::init() {
 }
 
 
+
 // Set voltage to the pwm pin
-void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
+void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta, float Uc) {
   float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f);
   // limit the voltage in driver
   Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit);
@@ -78,4 +79,4 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
     duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
   // write to hardware
   _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params);
-}
\ No newline at end of file
+}
diff --git a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h
index e4b2ee4..e10b824 100644
--- a/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h
+++ b/firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h
@@ -45,7 +45,8 @@ class StepperDriver4PWM: public StepperDriver
      * @param Ua phase A voltage
      * @param Ub phase B voltage
     */
-    void setPwm(float Ua, float Ub) override;
+    void setPwm(float Ua, float Ub, float NOT_USED = NOT_SET) override;
+    // void setPwm(float Ua, float Ub) override;
 
   private:
         
diff --git a/firmware/lib/currentsense/InlineCurrentSenseSync.cpp b/firmware/lib/currentsense/InlineCurrentSenseSync.cpp
new file mode 100644
index 0000000..b835ee2
--- /dev/null
+++ b/firmware/lib/currentsense/InlineCurrentSenseSync.cpp
@@ -0,0 +1,255 @@
+#include "InlineCurrentSenseSync.h"
+#include "communication/SimpleFOCDebug.h"
+// InlineCurrentSensor constructor
+//  - shunt_resistor  - shunt resistor value
+//  - gain  - current-sense op-amp gain
+//  - phA   - A phase adc pin
+//  - phB   - B phase adc pin
+//  - phC   - C phase adc pin (optional)
+InlineCurrentSenseSync::InlineCurrentSenseSync(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    shunt_resistor = _shunt_resistor;
+    amp_gain  = _gain;
+    volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+};
+
+
+InlineCurrentSenseSync::InlineCurrentSenseSync(float _mVpA, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    volts_to_amps_ratio = 1000.0f / _mVpA; // mV to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+};
+
+
+
+// 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;
+    // configure ADC variables
+    params = _configureADCInline(drv_params,pinA,pinB,pinC);
+    // if init failed return fail
+    if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; 
+    _driverSyncLowSide(driver->params, params);
+    // calibrate zero offsets
+    calibrateOffsets();
+    // set the initialized flag
+    initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
+    // return success
+    return 1;
+}
+// Function finding zero offsets of the ADC
+void InlineCurrentSenseSync::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+    
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params);
+        if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params);
+        if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params);
+        _delay(1);
+    }
+    // calculate the mean offsets
+    if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
+    if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s InlineCurrentSenseSync::getPhaseCurrents(){
+    PhaseCurrent_s current;
+    current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
+    current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
+    return current;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int InlineCurrentSenseSync::driverAlign(float voltage){
+    
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    if (driver==nullptr) {
+        SIMPLEFOC_DEBUG("CUR: No driver linked!");
+        return 0;
+    }
+
+    if (!initialized) return 0;
+
+    if(_isset(pinA)){
+        // set phase A active and phases B and C down
+        driver->setPwm(voltage, 0, 0);
+        _delay(2000);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the current 100 times ( arbitrary number )
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6f + 0.4f*c1.a;
+            c.b = c.b*0.6f + 0.4f*c1.b;
+            c.c = c.c*0.6f + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        // align phase A
+        float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
+        float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+        if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
+            gain_a *= _sign(c.a);
+        }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
+            gain_a *= _sign(c.a);
+        }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and B
+            int tmp_pinA = pinA;
+            pinA = pinB;
+            pinB = tmp_pinA;
+            float tmp_offsetA = offset_ia;
+            offset_ia = offset_ib;
+            offset_ib = tmp_offsetA;
+            gain_a *= _sign(c.b);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and C
+            int tmp_pinA = pinA;
+            pinA = pinC;
+            pinC= tmp_pinA;
+            float tmp_offsetA = offset_ia;
+            offset_ia = offset_ic;
+            offset_ic = tmp_offsetA;
+            gain_a *= _sign(c.c);
+            exit_flag = 2;// signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }
+    }
+
+    if(_isset(pinB)){
+        // set phase B active and phases A and C down
+        driver->setPwm(0, voltage, 0);
+        _delay(200);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the current 50 times
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6 + 0.4f*c1.a;
+            c.b = c.b*0.6 + 0.4f*c1.b;
+            c.c = c.c*0.6 + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
+        float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+        if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
+            gain_b *= _sign(c.b);
+        }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
+            gain_b *= _sign(c.b);
+        }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
+            // switch phase A and B
+            int tmp_pinB = pinB;
+            pinB = pinA;
+            pinA = tmp_pinB;
+            float tmp_offsetB = offset_ib;
+            offset_ib = offset_ia;
+            offset_ia = tmp_offsetB;
+            gain_b *= _sign(c.a);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and C
+            int tmp_pinB = pinB;
+            pinB = pinC;
+            pinC = tmp_pinB;
+            float tmp_offsetB = offset_ib;
+            offset_ib = offset_ic;
+            offset_ic = tmp_offsetB;
+            gain_b *= _sign(c.c);
+            exit_flag = 2; // signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }   
+    }
+
+    // if phase C measured
+    if(_isset(pinC)){
+        // set phase C active and phases A and B down
+        driver->setPwm(0, 0, voltage);
+        _delay(200);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the adc voltage 500 times ( arbitrary number )
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6 + 0.4f*c1.a;
+            c.b = c.b*0.6 + 0.4f*c1.b;
+            c.c = c.c*0.6 + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
+        float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
+        if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
+            gain_c *= _sign(c.c);
+        }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
+            gain_c *= _sign(c.c);
+        }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
+            // switch phase A and C
+            int tmp_pinC = pinC;
+            pinC = pinA;
+            pinA = tmp_pinC;
+            float tmp_offsetC = offset_ic;
+            offset_ic = offset_ia;
+            offset_ia = tmp_offsetC;
+            gain_c *= _sign(c.a);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
+            // switch phase B and C
+            int tmp_pinC = pinC;
+            pinC = pinB;
+            pinB = tmp_pinC;
+            float tmp_offsetC = offset_ic;
+            offset_ic = offset_ib;
+            offset_ib = tmp_offsetC;
+            gain_c *= _sign(c.b);
+            exit_flag = 2; // signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }   
+    }
+
+    if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+
+    return exit_flag;
+}
diff --git a/firmware/lib/currentsense/InlineCurrentSenseSync.h b/firmware/lib/currentsense/InlineCurrentSenseSync.h
new file mode 100644
index 0000000..43798e2
--- /dev/null
+++ b/firmware/lib/currentsense/InlineCurrentSenseSync.h
@@ -0,0 +1,75 @@
+#ifndef INLINE_CS_SYNC_LIB_H
+#define INLINE_CS_SYNC_LIB_H
+
+#include "Arduino.h"
+#include "SimpleFOC.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+#include "common/base_classes/CurrentSense.h"
+#include "common/lowpass_filter.h"
+// #include "current_sense/hardware_api.h"
+#include "utils.h"
+
+
+class InlineCurrentSenseSync: public CurrentSense{
+  public:
+    /**
+      InlineCurrentSense class constructor
+      @param shunt_resistor shunt resistor value
+      @param gain current-sense op-amp gain
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    InlineCurrentSenseSync(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
+    /**
+      InlineCurrentSense class constructor
+      @param mVpA mV per Amp ratio
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    InlineCurrentSenseSync(float mVpA, int pinA, int pinB, int pinC = NOT_SET);
+
+    // CurrentSense interface implementing functions 
+    int init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverAlign(float align_voltage) override;
+
+    // ADC measuremnet gain for each phase
+    // support for different gains for different phases of more commonly - inverted phase currents
+    // this should be automated later
+    float gain_a; //!< phase A gain
+    float gain_b; //!< phase B gain
+    float gain_c; //!< phase C gain
+
+    // // per phase low pass fileters
+    // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
+
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+    
+  private:
+  
+    // hardware variables
+  	int pinA; //!< pin A analog pin for current measurement
+  	int pinB; //!< pin B analog pin for current measurement
+  	int pinC; //!< pin C analog pin for current measurement
+
+    // gain variables
+    float shunt_resistor; //!< Shunt resistor value
+    float amp_gain; //!< amp gain value
+    float volts_to_amps_ratio; //!< Volts to amps ratio
+    
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+
+};
+
+#endif
diff --git a/firmware/lib/currentsense/adc.cpp b/firmware/lib/currentsense/adc.cpp
index cf9aea3..a1bfd86 100644
--- a/firmware/lib/currentsense/adc.cpp
+++ b/firmware/lib/currentsense/adc.cpp
@@ -5,8 +5,6 @@ ADC_HandleTypeDef hadc2;
 DMA_HandleTypeDef hdma_adc1;
 DMA_HandleTypeDef hdma_adc2;
 
-uint32_t HAL_RCC_ADC12_CLK_ENABLED = 0;
-
 void MX_ADC1_Init(void)
 {
   ADC_MultiModeTypeDef multimode = {0};
@@ -21,13 +19,17 @@ void MX_ADC1_Init(void)
   hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
   hadc1.Init.LowPowerAutoWait = DISABLE;
   hadc1.Init.ContinuousConvMode = DISABLE;
-  hadc1.Init.NbrOfConversion = 1;
+  hadc1.Init.NbrOfConversion = 3;
   hadc1.Init.DiscontinuousConvMode = DISABLE;
-  hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
+  hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T2_TRGO;
   hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
   hadc1.Init.DMAContinuousRequests = ENABLE;
   hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
   hadc1.Init.OversamplingMode = DISABLE;
+  hadc1.Init.Oversampling.Ratio = ADC_OVERSAMPLING_RATIO_16;
+  hadc1.Init.Oversampling.RightBitShift = ADC_RIGHTBITSHIFT_4;
+  hadc1.Init.Oversampling.TriggeredMode = ADC_TRIGGEREDMODE_SINGLE_TRIGGER;
+  hadc1.Init.Oversampling.OversamplingStopReset = ADC_REGOVERSAMPLING_CONTINUED_MODE;
   if (HAL_ADC_Init(&hadc1) != HAL_OK)
     SIMPLEFOC_DEBUG("HAL ADC1 Init fail.");
 
@@ -35,6 +37,7 @@ void MX_ADC1_Init(void)
   if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
     SIMPLEFOC_DEBUG("HAL ADC1 multimode configuration fail.");
 
+  // sConfig.Channel = ADC_CHANNEL_VOPAMP1;
   sConfig.Channel = ADC_CHANNEL_VOPAMP1;
   sConfig.Rank = ADC_REGULAR_RANK_1;
   sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
@@ -44,8 +47,13 @@ void MX_ADC1_Init(void)
   if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
     SIMPLEFOC_DEBUG("HAL ADC OPAMP1 init failed!");
 
-  sConfig.Channel = ADC_CHANNEL_TEMPSENSOR_ADC1;
+  sConfig.Channel = ADC_CHANNEL_3;
   sConfig.Rank = ADC_REGULAR_RANK_2;
+  if(HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+    SIMPLEFOC_DEBUG("HAL ADC Vmotor init failed!");
+
+  sConfig.Channel = ADC_CHANNEL_TEMPSENSOR_ADC1;
+  sConfig.Rank = ADC_REGULAR_RANK_3;
   if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
     SIMPLEFOC_DEBUG("HAL ADC temp init failed!");
 }
@@ -65,11 +73,15 @@ void MX_ADC2_Init(void)
   hadc2.Init.ContinuousConvMode = DISABLE;
   hadc2.Init.NbrOfConversion = 2;
   hadc2.Init.DiscontinuousConvMode = DISABLE;
-  hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
+  hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T2_TRGO;
   hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
   hadc2.Init.DMAContinuousRequests = ENABLE;
   hadc2.Init.Overrun = ADC_OVR_DATA_PRESERVED;
   hadc2.Init.OversamplingMode = DISABLE;
+  hadc2.Init.Oversampling.Ratio = ADC_OVERSAMPLING_RATIO_16;
+  hadc2.Init.Oversampling.RightBitShift = ADC_RIGHTBITSHIFT_4;
+  hadc2.Init.Oversampling.TriggeredMode = ADC_TRIGGEREDMODE_SINGLE_TRIGGER;
+  hadc2.Init.Oversampling.OversamplingStopReset = ADC_REGOVERSAMPLING_CONTINUED_MODE;
   if (HAL_ADC_Init(&hadc2) != HAL_OK)
     SIMPLEFOC_DEBUG("HAL ADC2 init failed!");
 
@@ -88,95 +100,51 @@ void MX_ADC2_Init(void)
     SIMPLEFOC_DEBUG("HAL ADC OPAMP3 init failed!");
 }
 
-void ADC_DMA_Init(ADC_HandleTypeDef* adcHandle)
+uint32_t HAL_RCC_ADC12_CLK_ENABLED = 0;
+
+void ADC_DMA_Init(void)
 {
-
   RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
-  if(adcHandle->Instance==ADC1)
+
+  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
+  PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
+  if(HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
+    SIMPLEFOC_DEBUG("Error initializing peripheral clock in adc.cpp");
+
+  HAL_RCC_ADC12_CLK_ENABLED++;
+  if (HAL_RCC_ADC12_CLK_ENABLED == 1)
   {
-    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
-    PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
-    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
-    {
-      Error_Handler();
-    }
-
-    HAL_RCC_ADC12_CLK_ENABLED++;
-    if(HAL_RCC_ADC12_CLK_ENABLED==1){
-      __HAL_RCC_ADC12_CLK_ENABLE();
-    }
-
-    /* ADC1 DMA Init */
-    /* ADC1 Init */
-    hdma_adc1.Instance = DMA1_Channel1;
-    hdma_adc1.Init.Request = DMA_REQUEST_ADC1;
-    hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY;
-    hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE;
-    hdma_adc1.Init.MemInc = DMA_MINC_ENABLE;
-    hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
-    hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
-    hdma_adc1.Init.Mode = DMA_NORMAL;
-    hdma_adc1.Init.Priority = DMA_PRIORITY_LOW;
-    if (HAL_DMA_Init(&hdma_adc1) != HAL_OK)
-    {
-      Error_Handler();
-    }
-
-    __HAL_LINKDMA(adcHandle,DMA_Handle,hdma_adc1);
+    __HAL_RCC_ADC12_CLK_ENABLE();
   }
-  else if(adcHandle->Instance==ADC2)
-  {
-    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
-    PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
-    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
-    {
-      Error_Handler();
-    }
 
-    HAL_RCC_ADC12_CLK_ENABLED++;
-    if(HAL_RCC_ADC12_CLK_ENABLED==1){
-      __HAL_RCC_ADC12_CLK_ENABLE();
-    }
+  /* ADC1 DMA Init */
+  hdma_adc1.Instance = DMA1_Channel1;
+  hdma_adc1.Init.Request = DMA_REQUEST_ADC1;
+  hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY;
+  hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE;
+  hdma_adc1.Init.MemInc = DMA_MINC_ENABLE;
+  hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+  hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+  hdma_adc1.Init.Mode = DMA_CIRCULAR;
+  hdma_adc1.Init.Priority = DMA_PRIORITY_LOW;
+  if (HAL_DMA_Init(&hdma_adc1) != HAL_OK)
+    SIMPLEFOC_DEBUG("HAL error enabling DMA1 channel 1.");
 
-    /* ADC2 DMA Init */
-    /* ADC2 Init */
-    hdma_adc2.Instance = DMA1_Channel2;
-    hdma_adc2.Init.Request = DMA_REQUEST_ADC2;
-    hdma_adc2.Init.Direction = DMA_PERIPH_TO_MEMORY;
-    hdma_adc2.Init.PeriphInc = DMA_PINC_DISABLE;
-    hdma_adc2.Init.MemInc = DMA_MINC_ENABLE;
-    hdma_adc2.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
-    hdma_adc2.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
-    hdma_adc2.Init.Mode = DMA_CIRCULAR;
-    hdma_adc2.Init.Priority = DMA_PRIORITY_LOW;
-    if (HAL_DMA_Init(&hdma_adc2) != HAL_OK)
-    {
-      Error_Handler();
-    }
+  __HAL_LINKDMA(&hadc1, DMA_Handle, hdma_adc1);
 
-    __HAL_LINKDMA(adcHandle,DMA_Handle,hdma_adc2);
-  }
+  /* ADC2 DMA Init */
+  hdma_adc2.Instance = DMA1_Channel2;
+  hdma_adc2.Init.Request = DMA_REQUEST_ADC2;
+  hdma_adc2.Init.Direction = DMA_PERIPH_TO_MEMORY;
+  hdma_adc2.Init.PeriphInc = DMA_PINC_DISABLE;
+  hdma_adc2.Init.MemInc = DMA_MINC_ENABLE;
+  hdma_adc2.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+  hdma_adc2.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+  hdma_adc2.Init.Mode = DMA_CIRCULAR;
+  hdma_adc2.Init.Priority = DMA_PRIORITY_LOW;
+  if (HAL_DMA_Init(&hdma_adc2) != HAL_OK)
+    SIMPLEFOC_DEBUG("HAL error enabling DMA1 channel 2.");
+
+  __HAL_LINKDMA(&hadc2, DMA_Handle, hdma_adc2);
 }
 
-// void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
-// {
-
-//   if(adcHandle->Instance==ADC1)
-//   {
-//     HAL_RCC_ADC12_CLK_ENABLED--;
-//     if(HAL_RCC_ADC12_CLK_ENABLED==0){
-//       __HAL_RCC_ADC12_CLK_DISABLE();
-//     }
-
-//     HAL_DMA_DeInit(adcHandle->DMA_Handle);
-//   }
-//   else if(adcHandle->Instance==ADC2)
-//   {
-//     HAL_RCC_ADC12_CLK_ENABLED--;
-//     if(HAL_RCC_ADC12_CLK_ENABLED==0){
-//       __HAL_RCC_ADC12_CLK_DISABLE();
-//     }
-
-//     HAL_DMA_DeInit(adcHandle->DMA_Handle);
-//   }
-// }
diff --git a/firmware/lib/currentsense/adc.h b/firmware/lib/currentsense/adc.h
index 62491b2..370f503 100644
--- a/firmware/lib/currentsense/adc.h
+++ b/firmware/lib/currentsense/adc.h
@@ -3,6 +3,7 @@
 
 #include "stm32g4xx_hal.h"
 #include "stm32g4xx_hal_adc.h"
+#include "stm32g4xx_hal_adc_ex.h"
 #include "communication/SimpleFOCDebug.h"
 
 extern ADC_HandleTypeDef hadc1;
@@ -12,7 +13,7 @@ extern DMA_HandleTypeDef hdma_adc2;
 
 void MX_ADC1_Init(void);
 void MX_ADC2_Init(void);
-void ADC_DMA_Init(ADC_HandleTypeDef* adcHandle);
+void ADC_DMA_Init(void);
 
 #endif 
 
diff --git a/firmware/lib/currentsense/dma.cpp b/firmware/lib/currentsense/dma.cpp
index 99a75df..996e762 100644
--- a/firmware/lib/currentsense/dma.cpp
+++ b/firmware/lib/currentsense/dma.cpp
@@ -4,7 +4,7 @@ void MX_DMA_Init(void)
 {
   __HAL_RCC_DMAMUX1_CLK_ENABLE();
   __HAL_RCC_DMA1_CLK_ENABLE();
-  __HAL_RCC_DMA2_CLK_ENABLE();
+  // __HAL_RCC_DMA2_CLK_ENABLE();
 
   HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
   HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
@@ -16,7 +16,6 @@ void MX_DMA_Init(void)
   PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
   PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
   HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
-
 }
 
 extern "C" {
diff --git a/firmware/lib/currentsense/opamp.cpp b/firmware/lib/currentsense/opamp.cpp
index e444c47..1559193 100644
--- a/firmware/lib/currentsense/opamp.cpp
+++ b/firmware/lib/currentsense/opamp.cpp
@@ -4,72 +4,73 @@ OPAMP_HandleTypeDef hopamp1;
 OPAMP_HandleTypeDef hopamp2;
 OPAMP_HandleTypeDef hopamp3;
 
-void initOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *opamp)
+void configureOPAMPs(void)
 {
-  hopamp1.Instance = opamp;
+  hopamp1.Instance = OPAMP1;
   hopamp1.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
-  hopamp1.Init.Mode = OPAMP_PGA_MODE;
+  hopamp1.Init.Mode = OPAMP_FOLLOWER_MODE;
   hopamp1.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO1;
   hopamp1.Init.InternalOutput = ENABLE;
   hopamp1.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
-  hopamp1.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO;
-  hopamp1.Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; // Adjust this to change the gains of the opamp.
   hopamp1.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
-  if (HAL_OPAMP_Init(hopamp) != HAL_OK)
-    SIMPLEFOC_DEBUG("HAL OPAMP Init failed!");
+  if(HAL_OPAMP_Init(&hopamp1) != HAL_OK)
+    SIMPLEFOC_DEBUG("OPAMP1 init failed.");
+
+  hopamp2.Instance = OPAMP2;
+  hopamp2.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
+  hopamp2.Init.Mode = OPAMP_FOLLOWER_MODE;
+  hopamp2.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO2;
+  hopamp2.Init.InternalOutput = ENABLE;
+  hopamp2.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
+  hopamp2.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
+  if(HAL_OPAMP_Init(&hopamp2) != HAL_OK)
+    SIMPLEFOC_DEBUG("OPAMP2 init failed.");
+
+  hopamp3.Instance = OPAMP3;
+  hopamp3.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
+  hopamp3.Init.Mode = OPAMP_FOLLOWER_MODE;
+  hopamp3.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO1;
+  hopamp3.Init.InternalOutput = ENABLE;
+  hopamp3.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
+  hopamp3.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
+  if(HAL_OPAMP_Init(&hopamp3) != HAL_OK)
+    SIMPLEFOC_DEBUG("OPAMP3 init failed.");
 }
 
-void configureOPAMPs(void)
+void OPAMP_GPIO_Init(void)
 {
-  initOPAMP(&hopamp1, OPAMP1); // PA3
-  initOPAMP(&hopamp2, OPAMP2); // PB0
-  initOPAMP(&hopamp3, OPAMP3); // PA1
-}
-
-void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* opampHandle)
-{
-
   GPIO_InitTypeDef GPIO_InitStruct = {0};
-  if(opampHandle->Instance==OPAMP1)
-  {
-    __HAL_RCC_GPIOA_CLK_ENABLE();
-    GPIO_InitStruct.Pin = GPIO_PIN_3;
-    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-  }
-  else if(opampHandle->Instance==OPAMP2)
-  {
-    __HAL_RCC_GPIOB_CLK_ENABLE();
-    GPIO_InitStruct.Pin = GPIO_PIN_0;
-    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-  }
-  else if(opampHandle->Instance==OPAMP3)
-  {
-    __HAL_RCC_GPIOA_CLK_ENABLE();
-    GPIO_InitStruct.Pin = GPIO_PIN_1;
-    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-  }
+
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  GPIO_InitStruct.Pin = GPIO_PIN_2;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  GPIO_InitStruct.Pin = GPIO_PIN_3;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  GPIO_InitStruct.Pin = GPIO_PIN_0;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  GPIO_InitStruct.Pin = GPIO_PIN_13;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 }
 
-void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* opampHandle)
+void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef *opampHandle)
 {
 
-  if(opampHandle->Instance==OPAMP1)
+  if (opampHandle->Instance == OPAMP1)
   {
     HAL_GPIO_DeInit(GPIOA, GPIO_PIN_3);
   }
-  else if(opampHandle->Instance==OPAMP2)
+  else if (opampHandle->Instance == OPAMP2)
   {
     HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0);
   }
-  else if(opampHandle->Instance==OPAMP3)
+  else if (opampHandle->Instance == OPAMP3)
   {
-    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1);
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_13);
   }
 }
-
diff --git a/firmware/lib/currentsense/opamp.h b/firmware/lib/currentsense/opamp.h
index 1ed5d22..ae14782 100644
--- a/firmware/lib/currentsense/opamp.h
+++ b/firmware/lib/currentsense/opamp.h
@@ -12,6 +12,7 @@ extern OPAMP_HandleTypeDef hopamp2;
 extern OPAMP_HandleTypeDef hopamp3;
 
 void configureOPAMPs(void);
+void OPAMP_GPIO_Init(void);
 
 #endif 
 
diff --git a/firmware/lib/currentsense/utils.cpp b/firmware/lib/currentsense/utils.cpp
index 244e466..92c6db5 100644
--- a/firmware/lib/currentsense/utils.cpp
+++ b/firmware/lib/currentsense/utils.cpp
@@ -7,10 +7,10 @@
 #include "opamp.h"
 
 float adcResolution = 4096.0f; // 12 bit ADC
-float voltageScale = 3.3f;     // full scale voltage range of ADC
-float adcSens = adcResolution * voltageScale;
+float voltageScale = 2.9f;     // full scale voltage range of ADC
+float adcSens =  voltageScale / adcResolution;
 
-volatile uint16_t adc1Result[2] = {0};
+volatile uint16_t adc1Result[3] = {0};
 volatile uint16_t adc2Result[2] = {0};
 
 void MX_GPIO_Init(void)
@@ -20,30 +20,36 @@ void MX_GPIO_Init(void)
     __HAL_RCC_GPIOA_CLK_ENABLE();
     __HAL_RCC_GPIOB_CLK_ENABLE();
     __HAL_RCC_GPIOC_CLK_ENABLE();
-
     __HAL_RCC_ADC12_CLK_ENABLE();
+
+    OPAMP_GPIO_Init();
 }
 
-float _readVoltageInline(const uint8_t pin, const void *cs_params)
+float _readADCVoltageInline(const int pin, const void *cs_params)
 {
-    uint32_t rawResult;
+    // SIMPLEFOC_DEBUG("READ ADC VOLTAGE");
+    uint16_t rawResult;
     switch (pin)
     {
-    case PA3:
-        rawResult = adc1Result[0]; // ADC1 CH13 -> Vopamp1 internal output
+    case PA3: 
+        rawResult = adc1Result[0];
+        break;
+
+    case PB13:
+        rawResult = adc2Result[0]; // ADC1 CH13 -> Vopamp1 internal output
         break;
 
     case PB0:
-        rawResult = adc2Result[0]; // ADC2 CH16 -> Vopamp2 internal output
+        rawResult = adc2Result[1]; // ADC2 CH16 -> Vopamp2 internal output
         break;
 
-    case PA1:
-        rawResult = adc2Result[1]; // ADC2 CH18 -> Vopamp3 internal output
-        break;
+    // case PA13:
+    //     rawResult = adc2Result[2]; // ADC2 CH18 -> Vopamp3 internal output
+    //     break;
 
-    case PA2:
-        rawResult = adc1Result[1]; // ADC1 CH16 -> not sure what pin should represent this?
-        break;
+    // case PA2:
+    //     rawResult = adc1Result[1]; // ADC1 CH16 -> not sure what pin should represent this?
+    //     break;
 
     default:
         return 0.0f;
@@ -60,19 +66,25 @@ 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();
+    HAL_SYSCFG_VREFBUF_VoltageScalingConfig(SYSCFG_VREFBUF_VOLTAGE_SCALE2);
+    HAL_SYSCFG_EnableVREFBUF();  
+    HAL_SYSCFG_VREFBUF_HighImpedanceConfig(SYSCFG_VREFBUF_HIGH_IMPEDANCE_DISABLE);
     MX_GPIO_Init();
     MX_DMA_Init();
     MX_ADC1_Init();
     MX_ADC2_Init();
     configureOPAMPs();
 
-    ADC_DMA_Init(&hadc1);
-    ADC_DMA_Init(&hadc2);
+    ADC_DMA_Init();
 
-    if (HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Result, 1) != HAL_OK)
+    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
+    HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
+
+    if (HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Result, 3) != HAL_OK)
     {
         SIMPLEFOC_DEBUG("DMA1 read init failed");
     }
@@ -106,18 +118,28 @@ 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;
 
     _stopTimers(driver_params->timers, 6);
 
     // See RM0440 pg. 1169
-    // This grabs the timer handle used for ADC and sets the direction bit as upcounting (?)
+    // Grab the timer handle used for ADC and sets the direction bit as upcounting
     cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
 
-    // This sets the value of the timer to the reload value. I think this is so that an event is immediately fired
+    /** Set the value of the timer to the reload value, so that overflow event is generated immediately.
+     * This is because we are using repetition counter to count every other event, skipping the underflow at 
+     * valleys of PWM. We can downsample every other peak if repetition counter is increased to 3
+     */
     cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
 
+    // Set TIM_CR1_URS to 0b1 -> only update events are overflows/underflow
+    cs_params->timer_handle->getHandle()->Instance->CR1 |= 0x0004;
+
+    // Set TIM_CR2_MMS to 0b010 -> update event (overflow and underflow) mapped to tim2_trgo
+    cs_params->timer_handle->getHandle()->Instance->CR2 |= 0x0020;
+
     _startTimers(driver_params->timers, 6);
 }
 
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/clock.c b/firmware/src/clock.c
index 0416025..fb14050 100644
--- a/firmware/src/clock.c
+++ b/firmware/src/clock.c
@@ -1,33 +1,33 @@
 #include "pins_arduino.h"
 
 /**
-  * @brief System Clock Configuration
-  * @retval None
-  */
+ * @brief System Clock Configuration
+ * @retval None
+ */
 void SystemClock_Config(void)
 {
   RCC_OscInitTypeDef RCC_OscInitStruct = {0};
   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+#ifdef USBCON
+  RCC_PeriphCLKInitTypeDef PeriphClkInit = {};
+#endif
 
   /** Configure the main internal regulator output voltage
-  */
+   */
   HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);
 
   /** Initializes the RCC Oscillators according to the specified parameters
-  * in the RCC_OscInitTypeDef structure.
-  */
-  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_HSI48
-                              |RCC_OSCILLATORTYPE_HSE;
+   * in the RCC_OscInitTypeDef structure.
+   */
+  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSE;
   RCC_OscInitStruct.HSEState = RCC_HSE_ON;
-  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
-  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
   RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
-  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
-  RCC_OscInitStruct.PLL.PLLN = 28;
+  RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV3;
+  RCC_OscInitStruct.PLL.PLLN = 85;
   RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
-  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV8;
+  RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
   RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
   {
@@ -35,9 +35,8 @@ void SystemClock_Config(void)
   }
 
   /** Initializes the CPU, AHB and APB buses clocks
-  */
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
-                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+   */
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
@@ -47,4 +46,13 @@ void SystemClock_Config(void)
   {
     Error_Handler();
   }
+
+  #ifdef USBCON
+  /* Initializes the peripherals clocks */
+  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
+  PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_HSI48;
+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) {
+    Error_Handler();
+  }
+#endif
 }
\ No newline at end of file
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index bffb6d2..d29066e 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"
@@ -13,6 +13,7 @@
 #include "can.h"
 #include "dfu.h"
 #include "utils.h"
+#include "InlineCurrentSenseSync.h"
 #include "lemon-pepper.h"
 
 #define USBD_MANUFACTURER_STRING "matei repair lab"
@@ -35,8 +36,8 @@ uint8_t updateData = 0;
 const uint16_t magicWord = 0xAF0C;
 
 // canbus things
-extern uint8_t TxData[8];
-extern uint8_t RxData[8];
+extern volatile uint8_t TxData[8];
+extern volatile uint8_t RxData[8];
 
 // simpleFOC things
 #define POLEPAIRS 50
@@ -44,6 +45,10 @@ 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 Serial3
+
+HardwareSerial Serial3 = HardwareSerial(PB8, PB9);
+
 /**
  * SPI clockdiv of 16 gives ~10.5MHz clock. May still be stable with lower divisor.
  * The HW encoder is configured using PPR, which is then *4 for CPR (full 12384 gives overflow on 16 bit timer.)
@@ -53,40 +58,58 @@ MagneticSensorMT6835 sensor = MagneticSensorMT6835(ENC_CS, myMT6835SPISettings);
 STM32HWEncoder enc = STM32HWEncoder(ENC_PPR, ENC_A, ENC_B, ENC_Z);
 
 /**
- * The current sense amps have a gain of 90mA/V -> over 1.5A this is 135mA so we need gain of 24 to get full-scale.
- * Actually we are limited to powers of 2 for gain. So it should be 16. This gives sensitivity of 1440mV/A.
- * */
-InlineCurrentSense currentsense = InlineCurrentSense(1440, ISENSE_U, ISENSE_V, ISENSE_W);
+ * TODO: Change the current sense code to reflect the new inline current sense amplifier choice with sense resistor.
+ */
+// InlineCurrentSenseSync currentsense = InlineCurrentSenseSync(90, ISENSE_U, ISENSE_V, ISENSE_W);
 
 StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2);
+// StepperMotor motor = StepperMotor(POLEPAIRS, RPHASE, MOTORKV, 0.0045);
 StepperMotor motor = StepperMotor(POLEPAIRS);
-Commander commander = Commander(SerialUSB);
+Commander commander = Commander(SERIALPORT);
 
 uint16_t counter = 0;
+extern volatile uint16_t adc1Result[3];
+extern volatile uint16_t adc2Result[2];
+
+DQCurrent_s foc_currents;
+float electrical_angle;
+PhaseCurrent_s phase_currents;
 
 // Prototypes
 uint8_t configureFOC(void);
 uint8_t configureCAN(void);
 uint8_t calibrateEncoder(void);
 
+void userButton(void);
+
 void setup()
 {
 	pinMode(LED_GOOD, OUTPUT);
 	pinMode(LED_FAULT, OUTPUT);
 	pinMode(CAL_EN, OUTPUT);
 	pinMode(MOT_EN, OUTPUT);
+	pinMode(USER_BUTTON, INPUT);
 
-	SerialUSB.begin(115200);
+	attachInterrupt(USER_BUTTON, userButton, RISING);
+
+	SERIALPORT.begin(115200);
 
 	EEPROM.get(0, boardData);
 
 	digitalWrite(MOT_EN, HIGH);
 	digitalWrite(CAL_EN, LOW);
 
-	if (!configureCAN())
-		SIMPLEFOC_DEBUG("CAN init failed.");
-	if (!configureFOC())
+	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.");
+		digitalWrite(LED_FAULT, HIGH);
+	}
 
 	if (sensor.getABZResolution() != ENC_PPR) // Check that PPR of the encoder matches our expectation.
 	{
@@ -137,21 +160,16 @@ void loop()
 	motor.move();
 	commander.run();
 
-	if (counter == 0xFFF)
-	{
-		digitalToggle(LED_GOOD);
-		SerialUSB.print(sensor.getAngle());
-		SerialUSB.print("\t");
-		SerialUSB.print(enc.getAngle());
-		SerialUSB.print("\t");
-		SerialUSB.println(sensor.getABZResolution());
+	electrical_angle = motor.electricalAngle();
+	// phase_currents = currentsense.getPhaseCurrents();
+	// foc_currents = currentsense.getFOCCurrents(electrical_angle);
 
-		// SerialUSB.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle());
+	if(counter == 0xFFF){
+		digitalToggle(LED_GOOD);
+		// Serial.println(adc1Result[0]);
 		counter = 0;
 	}
-
-	counter++;
-
+counter++;
 #ifdef HAS_MONITOR
 	motor.monitor();
 #endif
@@ -168,7 +186,7 @@ uint8_t configureFOC(void)
 	commander.verbose = VerboseMode::machine_readable;
 
 #ifdef SIMPLEFOC_STM32_DEBUG
-	SimpleFOCDebug::enable(&SerialUSB);
+	SimpleFOCDebug::enable(&SERIALPORT);
 #endif
 
 	// Encoder initialization.
@@ -207,15 +225,17 @@ uint8_t configureFOC(void)
 	driver.init();
 
 	// Motor PID parameters.
-	motor.PID_velocity.P = 5;
-	motor.PID_velocity.I = 24;
-	motor.PID_velocity.D = 0.01;
+    motor.PID_velocity.P = 0.035;
+    motor.PID_velocity.I = 0.01;
+    motor.PID_velocity.D = 0.000;
+    motor.LPF_velocity.Tf = 0.004;
 	motor.PID_velocity.output_ramp = 750;
 	motor.PID_velocity.limit = 500;
-	motor.LPF_velocity.Tf = 4;
 
-	motor.P_angle.P = 600;
-	motor.P_angle.limit = 10000;
+	motor.P_angle.P = 350;
+    motor.P_angle.I = 8;
+    motor.P_angle.D = 0.3;
+    motor.LPF_angle = 0.001;
 	motor.LPF_angle.Tf = 0; // try to avoid
 
 	// Motor initialization.
@@ -227,7 +247,7 @@ uint8_t configureFOC(void)
 
 // Monitor initialization
 #ifdef HAS_MONITOR
-	motor.useMonitoring(SerialUSB);
+	motor.useMonitoring(SERIALPORT);
 	motor.monitor_start_char = 'M';
 	motor.monitor_end_char = 'M';
 	motor.monitor_downsample = 250;
@@ -236,10 +256,10 @@ uint8_t configureFOC(void)
 	motor.linkSensor(&sensor);
 	motor.linkDriver(&driver);
 
-	currentsense.linkDriver(&driver);
-	currentsense.init();
-
-	motor.linkCurrentSense(&currentsense);
+	// currentsense.linkDriver(&driver);
+	// int ret = currentsense.init();
+	// SERIALPORT.printf("Current Sense init result: %i\n", ret);
+	// motor.linkCurrentSense(&currentsense);
 
 	motor.target = 10;
 
@@ -283,13 +303,13 @@ uint8_t calibrateEncoder(void)
 	currentSettings.autocal_freq = 0x1;
 	sensor.setOptions4(currentSettings);
 
-	uint16_t calTime = micros();
-	while (calTime - micros() < 2000000)
+	uint32_t calTime = micros();
+	while ((micros() - calTime) < 2000000)
 	{
 		motor.loopFOC();
 		motor.move();
 
-		if (calTime - micros() > 2000)
+		if ((micros() -calTime) > 2000)
 		{
 			// after motor is spinning at constant speed, enable calibration.
 			digitalWrite(LED_GOOD, HIGH);
@@ -302,3 +322,9 @@ uint8_t calibrateEncoder(void)
 
 	return sensor.getCalibrationStatus();
 }
+
+void userButton(void)
+{
+	if(USB->DADDR != 0)
+		jump_to_bootloader();
+}
\ No newline at end of file
diff --git a/hardware/currentsense.kicad_sch b/hardware/currentsense.kicad_sch
index 2b0e501..5c23165 100644
--- a/hardware/currentsense.kicad_sch
+++ b/hardware/currentsense.kicad_sch
@@ -648,6 +648,10 @@
     (stroke (width 0) (type default))
     (uuid 729cc6be-8aa4-41f4-be98-ff91b8dfb517)
   )
+  (wire (pts (xy 148.59 127) (xy 157.48 127))
+    (stroke (width 0) (type default))
+    (uuid 72c65ad4-7171-4f7e-9fdb-92b5a6e36ed1)
+  )
   (wire (pts (xy 208.28 135.89) (xy 208.28 139.7))
     (stroke (width 0) (type default))
     (uuid 76cc1c25-d03c-4186-9987-57e46edfc330)
@@ -792,6 +796,10 @@
     (stroke (width 0) (type default))
     (uuid fef7ec77-0cf9-49f6-9253-634118909a52)
   )
+  (wire (pts (xy 148.59 91.44) (xy 157.48 91.44))
+    (stroke (width 0) (type default))
+    (uuid ff03524d-69d1-4f45-9362-ad5afe606fd8)
+  )
 
   (text "join at power connector" (at 189.23 127 0)
     (effects (font (size 1.27 1.27)) (justify left bottom))
@@ -841,6 +849,32 @@
     (uuid f27d6576-8dc6-4b35-a6ee-606d878ff90e)
   )
 
+  (symbol (lib_id "Device:R_Small_US") (at 146.05 127 90) (unit 1)
+    (in_bom yes) (on_board yes) (dnp no) (fields_autoplaced)
+    (uuid 084a4821-5263-43df-98a8-7fb0f03d0a22)
+    (property "Reference" "R703" (at 146.05 120.65 90)
+      (effects (font (size 1.27 1.27)))
+    )
+    (property "Value" "20R" (at 146.05 123.19 90)
+      (effects (font (size 1.27 1.27)))
+    )
+    (property "Footprint" "Resistor_SMD:R_0402_1005Metric" (at 146.05 127 0)
+      (effects (font (size 1.27 1.27)) hide)
+    )
+    (property "Datasheet" "~" (at 146.05 127 0)
+      (effects (font (size 1.27 1.27)) hide)
+    )
+    (pin "1" (uuid d71cd166-4c52-45d6-aaa3-2fa89ff75908))
+    (pin "2" (uuid cd22acb1-7602-45f3-97fa-03db382a1c81))
+    (instances
+      (project "lemon-pepper"
+        (path "/0306e2fa-4433-4288-91d9-65a3484207ad/41d11f3b-333a-47c0-a126-1f991ee11e83"
+          (reference "R703") (unit 1)
+        )
+      )
+    )
+  )
+
   (symbol (lib_id "power:PWR_FLAG") (at 193.04 135.89 90) (unit 1)
     (in_bom yes) (on_board yes) (dnp no) (fields_autoplaced)
     (uuid 08d81450-96ad-4d2c-9c7a-dc9ecb10ad44)
diff --git a/hardware/lemon-pepper.kicad_pcb b/hardware/lemon-pepper.kicad_pcb
index 1a1b2a9..676d3f6 100644
--- a/hardware/lemon-pepper.kicad_pcb
+++ b/hardware/lemon-pepper.kicad_pcb
@@ -3948,6 +3948,60 @@
     )
   )
 
+  (footprint "Resistor_SMD:R_0402_1005Metric" (layer "F.Cu")
+    (tstamp 95bca4c9-8b19-49ca-886b-ad8247c70844)
+    (at 151.75 80.575 180)
+    (descr "Resistor SMD 0402 (1005 Metric), square (rectangular) end terminal, IPC_7351 nominal, (Body size source: IPC-SM-782 page 72, https://www.pcb-3d.com/wordpress/wp-content/uploads/ipc-sm-782a_amendment_1_and_2.pdf), generated with kicad-footprint-generator")
+    (tags "resistor")
+    (property "Sheetfile" "currentsense.kicad_sch")
+    (property "Sheetname" "current sense")
+    (property "ki_description" "Resistor, small US symbol")
+    (property "ki_keywords" "r resistor")
+    (path "/41d11f3b-333a-47c0-a126-1f991ee11e83/7ea0b72c-9d2c-4584-ae0c-f1c3223e17c3")
+    (attr smd)
+    (fp_text reference "R702" (at 0 -1.17) (layer "F.SilkS") hide
+        (effects (font (size 1 1) (thickness 0.15)))
+      (tstamp 6247f1a6-0294-4105-931c-92275fb1acbf)
+    )
+    (fp_text value "5k" (at 0 1.17) (layer "F.Fab")
+        (effects (font (size 1 1) (thickness 0.15)))
+      (tstamp 642f1893-5ce5-413d-9338-36db9b299530)
+    )
+    (fp_text user "${REFERENCE}" (at 0 0) (layer "F.Fab")
+        (effects (font (size 0.26 0.26) (thickness 0.04)))
+      (tstamp f04b372a-aa46-4335-b283-077355704754)
+    )
+    (fp_line (start -0.153641 -0.38) (end 0.153641 -0.38)
+      (stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp d44eb6de-0228-4a23-8700-80099de6f412))
+    (fp_line (start -0.153641 0.38) (end 0.153641 0.38)
+      (stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp cd65f080-b41e-42da-841b-46c1e4551fa5))
+    (fp_line (start -0.93 -0.47) (end 0.93 -0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 62d252bc-0199-400c-962b-fd91f823379b))
+    (fp_line (start -0.93 0.47) (end -0.93 -0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 07b7d8eb-2037-47dd-9e90-c40d4ee11a72))
+    (fp_line (start 0.93 -0.47) (end 0.93 0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 46ab5a3a-88d6-4848-b791-26903365f95d))
+    (fp_line (start 0.93 0.47) (end -0.93 0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 51ec5b0d-5582-448c-96ef-8cb6e953e93f))
+    (fp_line (start -0.525 -0.27) (end 0.525 -0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 5e28671b-da8e-4f5d-835c-b69ae7c8cfc9))
+    (fp_line (start -0.525 0.27) (end -0.525 -0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp cdc0b3d3-45f5-406e-845e-3a19569b81d8))
+    (fp_line (start 0.525 -0.27) (end 0.525 0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp b4aade76-17cd-4f85-b3f9-af5de67d2c96))
+    (fp_line (start 0.525 0.27) (end -0.525 0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 8623440b-838d-4c09-a92e-da5c344f18e0))
+    (pad "1" smd roundrect (at -0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
+      (net 89 "Net-(U702-VIOUT)") (pintype "passive") (tstamp c18b3391-9d17-444a-a37c-f9b574b9b512))
+    (pad "2" smd roundrect (at 0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
+      (net 17 "/current sense/V_SENSE") (pintype "passive") (tstamp 2499fba1-b026-4fce-b16c-c335e0e017c2))
+    (model "${KICAD6_3DMODEL_DIR}/Resistor_SMD.3dshapes/R_0402_1005Metric.wrl"
+      (offset (xyz 0 0 0))
+      (scale (xyz 1 1 1))
+      (rotate (xyz 0 0 0))
+    )
+  )
+
   (footprint "mechanical:NEMA14" locked (layer "F.Cu")
     (tstamp 95c5292d-b574-4ab1-ba30-310faa18e02f)
     (at 146.3 86.975)
@@ -6024,6 +6078,60 @@
     )
   )
 
+  (footprint "Resistor_SMD:R_0402_1005Metric" (layer "F.Cu")
+    (tstamp e69ae357-baf4-4ca2-a29c-a3d90a58930b)
+    (at 141.625 80.575 180)
+    (descr "Resistor SMD 0402 (1005 Metric), square (rectangular) end terminal, IPC_7351 nominal, (Body size source: IPC-SM-782 page 72, https://www.pcb-3d.com/wordpress/wp-content/uploads/ipc-sm-782a_amendment_1_and_2.pdf), generated with kicad-footprint-generator")
+    (tags "resistor")
+    (property "Sheetfile" "currentsense.kicad_sch")
+    (property "Sheetname" "current sense")
+    (property "ki_description" "Resistor, small US symbol")
+    (property "ki_keywords" "r resistor")
+    (path "/41d11f3b-333a-47c0-a126-1f991ee11e83/54a1fb90-bae5-4dd5-8295-b6d0f7fa3ae2")
+    (attr smd)
+    (fp_text reference "R701" (at 0 -1.17) (layer "F.SilkS") hide
+        (effects (font (size 1 1) (thickness 0.15)))
+      (tstamp 84555ffc-f97f-4e2f-8445-c17efa744be1)
+    )
+    (fp_text value "5k" (at 0 1.17) (layer "F.Fab")
+        (effects (font (size 1 1) (thickness 0.15)))
+      (tstamp a9d736e7-0902-4bf8-88ca-1e3cd6add12d)
+    )
+    (fp_text user "${REFERENCE}" (at 0 0) (layer "F.Fab")
+        (effects (font (size 0.26 0.26) (thickness 0.04)))
+      (tstamp 15d2c22e-0045-4429-9b25-506eb887bde5)
+    )
+    (fp_line (start -0.153641 -0.38) (end 0.153641 -0.38)
+      (stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp 5a66ac82-6db9-43ca-b684-c72e5afc7def))
+    (fp_line (start -0.153641 0.38) (end 0.153641 0.38)
+      (stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp acfb1a97-46ed-4da5-8d73-c3ecafb9650c))
+    (fp_line (start -0.93 -0.47) (end 0.93 -0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 7ce96ed2-9552-4191-89d8-2e72893cbd45))
+    (fp_line (start -0.93 0.47) (end -0.93 -0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 7fa0c713-2d68-4d7d-8c76-f3f09fcd26ef))
+    (fp_line (start 0.93 -0.47) (end 0.93 0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp de148a6d-f502-41c2-aa7c-a7abefca156b))
+    (fp_line (start 0.93 0.47) (end -0.93 0.47)
+      (stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 8ab22f38-253d-42a6-a5e7-a298bc69cd2a))
+    (fp_line (start -0.525 -0.27) (end 0.525 -0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp bf5fe7db-4045-4d45-bfb3-8e8251d79bdc))
+    (fp_line (start -0.525 0.27) (end -0.525 -0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 5f3f064d-940f-4e87-b3cc-ad14fae28ed3))
+    (fp_line (start 0.525 -0.27) (end 0.525 0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 092d6890-db08-4c9f-9651-658bb81d65ba))
+    (fp_line (start 0.525 0.27) (end -0.525 0.27)
+      (stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 50bbe285-0061-4deb-880b-6572d06e334a))
+    (pad "1" smd roundrect (at -0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
+      (net 88 "Net-(U701-VIOUT)") (pintype "passive") (tstamp 4aa895f7-e80a-4a4e-b224-5d13b7e92f23))
+    (pad "2" smd roundrect (at 0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
+      (net 16 "/current sense/U_SENSE") (pintype "passive") (tstamp fd1d151d-85f9-42b9-b634-e3adabe80824))
+    (model "${KICAD6_3DMODEL_DIR}/Resistor_SMD.3dshapes/R_0402_1005Metric.wrl"
+      (offset (xyz 0 0 0))
+      (scale (xyz 1 1 1))
+      (rotate (xyz 0 0 0))
+    )
+  )
+
   (footprint "Resistor_SMD:R_0402_1005Metric" (layer "F.Cu")
     (tstamp eff89581-ca64-4897-b991-183a6047b0b7)
     (at 149.625 91.75)
@@ -7418,11 +7526,13 @@
   (segment (start 155.91038 94.385) (end 156.395 94.385) (width 0.25) (layer "F.Cu") (net 1) (tstamp e2bd038f-6de5-487d-950e-026bc6461a01))
   (segment (start 136.575 76.395) (end 136.61 76.395) (width 0.25) (layer "F.Cu") (net 1) (tstamp e43385e3-da36-4f95-be0c-a8a3ae4b8711))
   (segment (start 141.25 103.75) (end 140.44283 102.94283) (width 0.25) (layer "F.Cu") (net 1) (tstamp e4454056-1b5c-4fe8-8d26-a1180a03800a))
+  (segment (start 146.625 83.64038) (end 147.095 83.17038) (width 0.25) (layer "F.Cu") (net 1) (tstamp e4719c62-c6da-41ca-9408-7e6930f33b77))
   (segment (start 152.472649 82.344096) (end 153.3 83.171447) (width 0.25) (layer "F.Cu") (net 1) (tstamp e4a9dff8-4710-4c5e-8b26-a683cf6f4897))
   (segment (start 155.425 84.225) (end 154 84.225) (width 0.25) (layer "F.Cu") (net 1) (tstamp e64208e8-a8cb-4e94-a5b2-dbf8b34295c6))
   (segment (start 147.775 93.1) (end 149.31 93.1) (width 0.25) (layer "F.Cu") (net 1) (tstamp e70c6643-48cc-4c3a-9c9c-8aaa2ae8253b))
   (segment (start 142.0605 86.925) (end 142.05 86.9145) (width 0.25) (layer "F.Cu") (net 1) (tstamp e75bdc4e-838b-402f-905a-e2f2c94b3d38))
   (segment (start 154.59 97.295) (end 154.59 97.39) (width 0.25) (layer "F.Cu") (net 1) (tstamp e8dbc1e0-802e-494b-8b9b-105e03bf313d))
+  (segment (start 146.625 84.2) (end 146.625 83.64038) (width 0.25) (layer "F.Cu") (net 1) (tstamp e93d71b6-80da-43c4-8d73-ca538df56daf))
   (segment (start 138.76 91.84) (end 138.76 91.04) (width 0.25) (layer "F.Cu") (net 1) (tstamp e9d2e006-b8c3-44df-827c-6e0e3dedaa32))
   (segment (start 147.275 83.43) (end 147.275 84.2) (width 0.25) (layer "F.Cu") (net 1) (tstamp ea4475b3-0d49-4cfa-82fc-cfa5a51cd07a))
   (segment (start 136.388533 93.008293) (end 136.883507 93.008293) (width 0.25) (layer "F.Cu") (net 1) (tstamp eb4cc686-22a8-4de1-af9f-411e18b7abe3))
diff --git a/hardware/lemon-pepper.kicad_prl b/hardware/lemon-pepper.kicad_prl
index b051c13..d6cba81 100644
--- a/hardware/lemon-pepper.kicad_prl
+++ b/hardware/lemon-pepper.kicad_prl
@@ -1,6 +1,6 @@
 {
   "board": {
-    "active_layer": 2,
+    "active_layer": 0,
     "active_layer_preset": "",
     "auto_track_width": true,
     "hidden_netclasses": [],
diff --git a/platformio.ini b/platformio.ini
index bb92d16..cf6ccdb 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -10,7 +10,6 @@
 
 [platformio]
 default_envs = lemon-pepper
-boards_dir = firmware/boards
 src_dir = firmware/src
 lib_dir = firmware/lib
 include_dir = firmware/include
@@ -19,7 +18,7 @@ test_dir = firmware/test
 [env:lemon-pepper]
 platform = ststm32
 board = genericSTM32G431CB
-board_build.f_cpu = 168000000
+; board_build.f_cpu = 170000000
 framework = arduino
 upload_protocol = stlink
 debug_tool = stlink
@@ -34,10 +33,12 @@ build_flags =
 	-D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC
 	-D HAL_FDCAN_MODULE_ENABLED
 	-D HAL_OPAMP_MODULE_ENABLED
+	-D HSE_VALUE=12000000U
 	-D FDCAN_ALT1
 	-D SN65HVD23x
 	-D ARDUINO_GENERIC_G431CBUX
 	-D SIMPLEFOC_STM32_CUSTOMCURRENTSENSE
+	-D LEMONPEPPER
 	; -D PCB_REV1 ; or PCB_REV2
 	; -D HAS_MONITOR