diff --git a/Main/IMU.ino b/Main/IMU.ino
index 4869cba..772ed37 100644
--- a/Main/IMU.ino
+++ b/Main/IMU.ino
@@ -35,7 +35,7 @@ void readIMU() {
     az = az_;
   }
 
-  acc_pitch = -1*atan2(sqrt(ay^2+az^2),ax);
+  acc_pitch = -1*atan2(ax,sqrt((pow(ay,2)+pow(az,2))))*180/3.14;
   
 
 //Serial plotter
@@ -44,13 +44,13 @@ void readIMU() {
 //  map(az,-140,140,-1,1)
 
   //    Serial.print  ( " x:" );
-  Serial.print  ( ax );
-  Serial.print  ( "," );
-  Serial.print  ( ay );
-  Serial.print  ( "," );
-  Serial.print  ( az );
-  Serial.print  ( "," );
-  Serial.println  ( acc_pitch*180/3.14 );
+//  Serial.print  ( ax );
+//  Serial.print  ( "," );
+//  Serial.print  ( ay );
+//  Serial.print  ( "," );
+//  Serial.print  ( az );
+//  Serial.print  ( "," );
+//  Serial.println  ( acc_pitch);
 
 
 
diff --git a/Main/Main.ino b/Main/Main.ino
index 9c73a14..d4e353c 100644
--- a/Main/Main.ino
+++ b/Main/Main.ino
@@ -10,6 +10,7 @@ int   ax, ay, az;
 int   cx, cy, cz;
 float gx, gy, gz, gt;
 float acc_pitch;
+signed int speed_setp;
 
 int i, modifier;
 
@@ -41,8 +42,8 @@ void setup() {
   ledcAttachPin(m2_in1, 3);
   ledcAttachPin(m2_in2, 4);
 
-  ledcSetup(1, 12000, 12); // 12 kHz PWM, 8-bit resolution
-  ledcSetup(2, 12000, 12);
+  ledcSetup(1, 12000, 8); // 12 kHz PWM, 8-bit resolution
+  ledcSetup(2, 12000, 8);
   ledcSetup(3, 12000, 8);
   ledcSetup(4, 12000, 8);
 
diff --git a/Main/motorControl.ino b/Main/motorControl.ino
index 2cf7f64..fed3feb 100644
--- a/Main/motorControl.ino
+++ b/Main/motorControl.ino
@@ -1,5 +1,19 @@
 void motorControl() {
-  //Motor 1
+//  //Motor 1
+//  speed_setp = round(map(acc_pitch, -90.0, 90.0, -255, 255));
+//  if (speed_setp > 0) {
+//    ledcWrite(1, speed_setp);
+//    ledcWrite(2, 0);
+//  }
+//  else if (speed_setp < 0) {
+//    ledcWrite(1, 0);
+//    ledcWrite(2, -1*speed_setp);
+//  }
+//  Serial.print  (acc_pitch);
+//  Serial.print  ( "," );
+//  Serial.println(speed_setp);
+
+
   ledcWrite(1, 0);
   ledcWrite(2, 0);