diff --git a/firmware/lib/currentsense/adc.cpp b/firmware/lib/currentsense/adc.cpp
index a9f6e68..a1bfd86 100644
--- a/firmware/lib/currentsense/adc.cpp
+++ b/firmware/lib/currentsense/adc.cpp
@@ -26,6 +26,10 @@ void MX_ADC1_Init(void)
   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.");
 
@@ -74,6 +78,10 @@ void MX_ADC2_Init(void)
   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!");
 
diff --git a/firmware/lib/currentsense/opamp.cpp b/firmware/lib/currentsense/opamp.cpp
index 2200170..1559193 100644
--- a/firmware/lib/currentsense/opamp.cpp
+++ b/firmware/lib/currentsense/opamp.cpp
@@ -26,7 +26,7 @@ void configureOPAMPs(void)
   if(HAL_OPAMP_Init(&hopamp2) != HAL_OK)
     SIMPLEFOC_DEBUG("OPAMP2 init failed.");
 
-  hopamp3.Instance = OPAMP2;
+  hopamp3.Instance = OPAMP3;
   hopamp3.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
   hopamp3.Init.Mode = OPAMP_FOLLOWER_MODE;
   hopamp3.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO1;
diff --git a/firmware/lib/currentsense/utils.cpp b/firmware/lib/currentsense/utils.cpp
index 2a66482..92c6db5 100644
--- a/firmware/lib/currentsense/utils.cpp
+++ b/firmware/lib/currentsense/utils.cpp
@@ -7,8 +7,8 @@
 #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[3] = {0};
 volatile uint16_t adc2Result[2] = {0};
@@ -28,24 +28,24 @@ void MX_GPIO_Init(void)
 float _readADCVoltageInline(const int pin, const void *cs_params)
 {
     // SIMPLEFOC_DEBUG("READ ADC VOLTAGE");
-    uint32_t rawResult;
+    uint16_t rawResult;
     switch (pin)
     {
-    case PA2: 
-        rawResult = adc1Result[1];
+    case PA3: 
+        rawResult = adc1Result[0];
         break;
 
-    case PA3:
-        rawResult = adc1Result[0]; // ADC1 CH13 -> Vopamp1 internal output
+    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 PA13:
-        rawResult = adc2Result[2]; // 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?
diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index 4fd684c..9ab5218 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -61,9 +61,12 @@ 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.
  * */
-InlineCurrentSenseSync currentsense = InlineCurrentSenseSync(1440, ISENSE_U, ISENSE_V);
+InlineCurrentSenseSync currentsense = InlineCurrentSenseSync(90, ISENSE_U, ISENSE_V, ISENSE_W);
 
 StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2);
+// StepperDriver4PWM driver = StepperDriver4PWM(PA0, PA9, PA1, PA10, PB12);
+// 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(SERIALPORT);
 
@@ -145,17 +148,26 @@ void setup()
 	// }
 }
 
+DQCurrent_s foc_currents;
+float electrical_angle;
+PhaseCurrent_s phase_currents;
+
 void loop()
 {
 	motor.loopFOC();
 	motor.move();
 	commander.run();
 
-	if(counter == 0){
-		digitalToggle(LED_GOOD);
-		Serial.println(adc1Result[0]);
-	}
+	electrical_angle = motor.electricalAngle();
+	phase_currents = currentsense.getPhaseCurrents();
+	foc_currents = currentsense.getFOCCurrents(electrical_angle);
 
+	if(counter == 0xFFF){
+		digitalToggle(LED_GOOD);
+		// Serial.println(adc1Result[0]);
+		counter = 0;
+	}
+counter++;
 #ifdef HAS_MONITOR
 	motor.monitor();
 #endif
@@ -211,15 +223,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.