diff --git a/Main/IMU.ino b/Main/IMU.ino
index 772ed37..27a593a 100644
--- a/Main/IMU.ino
+++ b/Main/IMU.ino
@@ -1,3 +1,11 @@
+//IMU variables
+
+int   ax_, ay_, az_;
+int   ax, ay, az;
+int   cx, cy, cz;
+float gx, gy, gz, gt;
+float acc_pitch;
+
 void readIMU() {
   ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() );
   ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() );
@@ -43,12 +51,12 @@ void readIMU() {
 //  map(ay,-140,140,-1,1)
 //  map(az,-140,140,-1,1)
 
-  //    Serial.print  ( " x:" );
+//      Serial.print  ( " x:" );
 //  Serial.print  ( ax );
 //  Serial.print  ( "," );
 //  Serial.print  ( ay );
 //  Serial.print  ( "," );
-//  Serial.print  ( az );
+//  Serial.println  ( az );
 //  Serial.print  ( "," );
 //  Serial.println  ( acc_pitch);
 
diff --git a/Main/Main.ino b/Main/Main.ino
index b6f2822..06d2cc8 100644
--- a/Main/Main.ino
+++ b/Main/Main.ino
@@ -2,21 +2,10 @@
 #include "GY_85.h"
 #include <Wire.h>
 
-//System variables
-unsigned long now = micros();
-
-
-//IMU variables
 GY_85 GY85;     //create the object
-int   ax_, ay_, az_;
-int   ax, ay, az;
-int   cx, cy, cz;
-float gx, gy, gz, gt;
-float acc_pitch;
-signed int speed_setp;
 
-//Random
-int i, modifier;
+//System variables
+unsigned long tNow = micros();
 
 //Motor Control variables
 const byte m1_in1 = 18;
@@ -25,7 +14,7 @@ const byte m1_in2 = 19;
 //int m2_in2 = 17;
 
 //Encoders variables
-long int m1Raw, m1RawLast;
+long int m1Raw,m1RawLast;
 const byte pin_m1_A = 34;
 const byte pin_m1_B = 35;
 volatile bool A_state;
@@ -124,20 +113,19 @@ void setup() {
   //  ledcSetup(3, 12000, 8);
   //  ledcSetup(4, 12000, 8);
 
-  //Random
-  i = 0;
-  modifier = 2;
+Serial.println("Reference,Actual,SpeedCommand");
+
 }
 
 void loop() {
   //Update system variables
-  now = micros();
+  tNow = micros();
 
-  //Only print encoder value if value changed since last print
-  if (m1Raw != m1RawLast) {
-    Serial.println(m1Raw);
-    m1RawLast = m1Raw;
-  }
+////  //Only print encoder value if value changed since last print
+//  if (m1Raw != m1RawLast) {
+//    Serial.println(m1Raw);
+//    m1RawLast = m1Raw;
+//  }
 
   //Sense
   readIMU();
diff --git a/Main/motorControl.ino b/Main/motorControl.ino
index c7e57be..89189ed 100644
--- a/Main/motorControl.ino
+++ b/Main/motorControl.ino
@@ -1,13 +1,47 @@
+//Constants
+const float wheel_diameter  = 0.067708;
+const float pulses_per_turn   = 1320.0;
+//Tuning
+const float K = 2;
+const float I = 10;
+
+//Help variables
+int         dEnc, dT;
+float       dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered;
+unsigned long tLast;
+
+float speed_setp,referenceSpeed,actualSpeed;
+float error, iError;
+int speedCMD;
+
+
 void motorControl() {
   //Motor 1
-  speed_setp = round (map(acc_pitch, -90.0, 90.0, -4096, 4096));
-  if (speed_setp > 0) {
-    ledcWrite(1, speed_setp);
-    ledcWrite(2, 0);
+  speed_setp = map(acc_pitch, -90.0, 90.0, -4096, 4096);
+  referenceSpeed = speed_setp;
+  actualSpeed = lin_vel*4096;
+  error = referenceSpeed - actualSpeed;
+  iError = iError + (error*dT*pow(10,-6)*I);
+  speedCMD = round((error*K)+iError);
+
+//  Saturation
+  if(speedCMD>4096){
+    speedCMD = 4096;
   }
-  else if (speed_setp < 0) {
+  else if(speedCMD < -4096){
+    speedCMD = -4096;
+  }
+  else{
+    speedCMD = speedCMD;
+  }
+  
+  if (speedCMD > 0) {
     ledcWrite(1, 0);
-    ledcWrite(2, -1 * speed_setp);
+    ledcWrite(2, speedCMD);
+  }
+  else if (speedCMD < 0) {
+    ledcWrite(1, -1 * speedCMD);
+    ledcWrite(2, 0);
   }
   //  Serial.print  (acc_pitch);
   //  Serial.print  ( "," );
@@ -21,4 +55,28 @@ void motorControl() {
   //Motor 2
   //  ledcWrite(3, 255);
   //  ledcWrite(4, 255);
+
+  //Calculate speed from encoders
+  dEnc  = m1Raw - m1RawLast; //[pulses]
+  dT    = tNow - tLast;     //[micro sec]
+  dTurn = dEnc / pulses_per_turn;
+
+  ang_vel = (dTurn * 2 * PI) /(dT*0.000001);
+  lin_vel = (dTurn * wheel_diameter * PI) /(dT*0.000001);
+
+  Serial.print(referenceSpeed);
+  Serial.print  ( "," );
+  Serial.println(actualSpeed);
+//  Serial.print  ( "," );
+//  Serial.println(speedCMD);
+//  Serial.print  ( "," );
+//  Serial.println(lin_vel);
+//  Serial.print  ( "," );
+
+  //update variables for next scan cycle
+  m1RawLast = m1Raw;
+  tLast = tNow;
+
+
+
 }