diff --git a/Inc/config.h b/Inc/config.h
index 8bfcd14..27e0ffc 100644
--- a/Inc/config.h
+++ b/Inc/config.h
@@ -13,12 +13,13 @@
 #define PWM_FREQ         16000     // PWM frequency in Hz
 #define DEAD_TIME        32        // PWM deadtime
 
-#define DC_CUR_LIMIT     34         // Motor DC current limit in amps
+//#define DC_CUR_LIMIT     34         // Motor DC current limit in amps
+#define DC_CUR_LIMIT     24         // Motor DC current limit in amps
 
 //#define DEBUG_SERIAL_SERVOTERM
-#define DEBUG_SERIAL_ASCII
+// #define DEBUG_SERIAL_ASCII
 #define DEBUG_BAUD       115200    // UART baud rate
 //#define DEBUG_I2C_LCD
 
-#define CONTROL_PPM                // use PPM CONTROL_PPM
+// #define CONTROL_PPM                // use PPM CONTROL_PPM
 #define PPM_NUM_CHANNELS 6         // number of PPM channels to receive
diff --git a/Src/bldc.c b/Src/bldc.c
index f694274..613e3fd 100644
--- a/Src/bldc.c
+++ b/Src/bldc.c
@@ -9,6 +9,7 @@ volatile int posl = 0;
 volatile int posr = 0;
 volatile int pwml = 0;
 volatile int pwmr = 0;
+float pwmrl = 0;
 
 extern volatile adc_buf_t adc_buffer;
 
@@ -117,11 +118,7 @@ inline void blockPhaseCurrent(int pos, int u, int v, int *q) {
   }
 }
 
-int last_pos     = 0;
-int timer        = 0;
 uint16_t buzzerTimer        = 0;
-int max_time     = PWM_FREQ / 10;
-volatile int vel = 0;
 
 int offsetcount = 0;
 int offsetrl1   = 2000;
@@ -132,12 +129,19 @@ int offsetdcl   = 2000;
 int offsetdcr   = 2000;
 
 float batteryVoltage;
+float adccmd1;
+float adccmd2;
 
 int curl = 0;
 // int errorl = 0;
 // int kp = 5;
 // volatile int cmdl = 0;
 
+int last_pos = 0;
+int timer = 0;
+const int max_time = PWM_FREQ / 10;
+volatile int vel = 0;
+
 void DMA1_Channel1_IRQHandler() {
   DMA1->IFCR = DMA_IFCR_CTCIF1;
   // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
@@ -154,6 +158,21 @@ void DMA1_Channel1_IRQHandler() {
   }
 
   batteryVoltage = batteryVoltage * 0.999 + ((float)adc_buffer.batt1 * ADC_BATTERY_VOLT) * 0.001;
+  adccmd1 = adccmd1 * 0.999 + (float)adc_buffer.l_rx2 * 0.001;
+  adccmd2 = adccmd2 * 0.999 + (float)adc_buffer.l_tx2 * 0.001;
+
+  //0-4096
+  //+-2000
+  
+  pwmrl = 0;
+  if(adccmd1 - 700 > 0){
+    pwmrl += adccmd1 - 700;
+  }
+  if(adccmd2 - 700 > 0){
+    pwmrl -= adccmd2 - 700;
+  }
+  pwml = -pwmrl/4;
+  pwmr = pwmrl/4;
 
   if(ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT || timeout > 50 || enable == 0) {
     LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
@@ -195,10 +214,8 @@ void DMA1_Channel1_IRQHandler() {
   setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
   setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
 
-  timer++;
   buzzerTimer++;
 
-
   if (buzzerFreq != 0 && (buzzerTimer / 1500) % (buzzerPattern + 1) == 0) {
     if (buzzerTimer % buzzerFreq == 0) {
       HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN);
@@ -207,22 +224,23 @@ void DMA1_Channel1_IRQHandler() {
       HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, 0);
   }
 
-
+  // //measure vel
+  // timer++;
   // if(timer > max_time){
   //   timer = max_time;
   //   vel = 0;
   // }
 
-  // if(pos != last_pos){
+  // if(posl != last_pos){
   //   vel = 1000 * PWM_FREQ / timer / P / 6 * 2;
 
-  //   if((pos - last_pos + 6) % 6 > 2){
+  //   if((posl - last_pos + 6) % 6 > 2){
   //     vel = -vel;
   //   }
 
   //   timer = 0;
   // }
-  // last_pos = pos;
+  // last_pos = posl;
 
   //YOLOTEST
   // errorl = cmdl - curl;
diff --git a/Src/main.c b/Src/main.c
index 5258b42..a930219 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -128,7 +128,7 @@ int main(void) {
 
     consoleScope();
 
-    timeout++;
+    timeout=0;
 
     if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
       enable = 0;
diff --git a/Src/setup.c b/Src/setup.c
index f93d494..29d7498 100644
--- a/Src/setup.c
+++ b/Src/setup.c
@@ -179,6 +179,12 @@ void MX_GPIO_Init(void) {
   GPIO_InitStruct.Pin = DCLINK_PIN;
   HAL_GPIO_Init(DCLINK_PORT, &GPIO_InitStruct);
 
+  //Analog in
+  GPIO_InitStruct.Pin = GPIO_PIN_3;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+  GPIO_InitStruct.Pin = GPIO_PIN_2;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
   GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
 
   GPIO_InitStruct.Pin = LEFT_TIM_UH_PIN;