diff --git a/witty.ino b/witty.ino
index 23bd6bb..48841b3 100644
--- a/witty.ino
+++ b/witty.ino
@@ -1,289 +1,289 @@
-#define DEBUG 1
-
-#include "I2Cdev.h"
-#include "MPU6050.h"
-#include "Wire.h"
-#define mRd 7
-#define mRa 6
-#define mLd 4
-#define mLa 5
-#define Gry_offset 0  //The offset of the gyro
-#define Gyr_Gain 8.000 // 16.348
-
-#define Angle_offset 0  // The offset of the accelerator
-float Angle_Delta, Angle_Recursive, Angle_Confidence;
-
-#define RMotor_offset_fwd 0 // The offset of the Motor
-#define LMotor_offset_fwd 0  // The offset of the Motor
-#define RMotor_offset_bck 0 // The offset of the Motor
-#define LMotor_offset_bck 0  // The offset of the Motor
-#define Mramp 3
-#define pi 3.14159
-float kp, ki, kd;
-float Angle_Raw, Angle_Filtered, omega;
-float Turn_Speed = 0, Turn_Speed_K = 0;
-float Run_Speed = 0, Run_Speed_K = 0;
-float LOutput, ROutput;
-float LlastA, RlastA;
-int LlastD, RlastD;
-unsigned long preTime = 0;
-unsigned long lastTime;
-float Input, Output;
-float errSum, dErr, error, lastErr;
-int timeChange;
-
-// class default I2C address is 0x68
-// specific I2C addresses may be passed as a parameter here
-// AD0 low = 0x68 (default for InvenSense evaluation board)
-// AD0 high = 0x69
-MPU6050 accelgyro;
-MPU6050 initialize;
-
-int16_t ax, ay, az;
-int16_t gx, gy, gz;
-
-
-void setup() {
-#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-  Wire.begin();
-#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
-  Fastwire::setup(400, true);
-#endif
-  Serial.begin(115200);
-  Serial.println("Initializing I2C devices...");
-  accelgyro.initialize();
-  Serial.println("Testing device connections...");
-  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
-  initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);;
-  for (int i = 0; i < 100; i++) // Looping 200 times to get the real gesture when starting
-  {
-    Filter();
-  }
-
-  pinMode(mRd, OUTPUT);
-  pinMode(mRa, OUTPUT);
-  pinMode(mLd, OUTPUT);
-  pinMode(mLa, OUTPUT);
-  LlastA = 0.0;
-  RlastA = 0.0;
-  LlastD = LOW;
-  RlastD = LOW;
-}
-
-void loop() {
-  // put your main code here, to run repeatedly:
-  Filter();
-#ifdef DEBUG
-//  Serial.print(" Raw =\t");
-//  Serial.print(Angle_Raw);
-  Serial.print(" Angle =\t");
-  Serial.print(Angle_Filtered);
-#endif
-  // If angle > 45 or < -45 then stop the robot
-  if (abs(Angle_Filtered) < 45)
-  {
-    // optional position to check external input
-    myPID();
-    PWMControl();
-  }
-  else
-  {
-    // stop wheels
-    digitalWrite(mRd, LOW);
-    analogWrite(mRa, 0);
-    digitalWrite(mLd, LOW);
-    analogWrite(mLa, 0);
-    for (int i = 0; i < 100; i++) // Keep reading the gesture after falling down
-    {
-      Filter();
-    }
-    if (abs(Angle_Filtered) < 45) // Empty datas and restart the robot automaticly
-    {
-      for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds
-      {
-        omega = Angle_Raw = Angle_Filtered = 0;
-        Filter();
-        Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
-        myPID();
-      }
-    }
-  }
-#ifdef DEBUG
-  Serial.println();
-  delay(DEBUG);
-#endif
-}
-
-void Filter()
-{
-  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-  //Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset);
-  Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset);
-  omega =  -gx / Gyr_Gain + Gry_offset;
-  // Filters datas to get the real gesture
-  unsigned long now = micros();
-  float dt = (now - preTime) * 0.000001;
-  preTime = now;
-  float K = 0.8;
-  float A = K / (K + dt);
-  Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64;
-  Angle_Recursive = Angle_Delta * dt + Angle_Recursive;
-  Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega;
-  Angle_Filtered = Angle_Confidence * dt + Angle_Filtered;
-  // Preparing datas which need to be shown on the LCD.
-}
-
-void myPID()
-{
-  kp = 1.000; // 22.000
-  ki = 0;
-  kd = 0.00; // 1.60
-  // Calculating the output values using the gesture values and the PID values.
-  error = Angle_Filtered;
-  errSum += error;
-  dErr = error - lastErr;
-  Output = kp * error + ki * errSum + kd * omega;
-  lastErr = error;
-#ifdef DEBUG
-//  Serial.print("\t error:\t");
- // Serial.print(error);
-//  Serial.print("\t errSum:\t");
-//  Serial.print(errSum);
-//  Serial.print("\t dErr:\t");
-//  Serial.print(dErr);
-//  Serial.print("\t omega:\t");
-//  Serial.print(omega);
-  Serial.print("\t output:\t");
-  Serial.print(Output);
-//  Output = 0;
-#endif
-  LOutput = Output - Turn_Speed + Run_Speed;
-  ROutput = Output + Turn_Speed + Run_Speed;
-}
-
-void PWMControl()
-{
-  // forward min 15 both
-  // reverse min 35 both
-  int sL, sR;
-
-  if (LOutput >= 0.0) {
-    // move forward
-    sL = min(255, LMotor_offset_fwd + LOutput);
-    if (LlastD == HIGH){
-      fade( mLa, LlastD, LOW, LlastA, 127);
-      digitalWrite(mLd, LOW);
-    }
-    fade( mLa, LlastD, LOW, LlastA, sL);
-    LlastD = LOW;
-    LlastA = sL;
-  }
-  if (LOutput < 0.0) {
-    sL = max(0, 255 - LMotor_offset_bck + LOutput);
-    if (LlastD == LOW){
-      fade( mLa, LlastD, HIGH, LlastA, 127);
-      digitalWrite(mLd, HIGH);
-    }
-    fade( mLa, LlastD, HIGH, LlastA, sL);
-    LlastD = HIGH;
-    LlastA = sL;
-  }
-  if (ROutput >= 0.0) {
-    sR = min(255, RMotor_offset_fwd + ROutput);
-    if (RlastD == HIGH){
-      fade( mRa, RlastD, LOW, RlastA, 127);
-      digitalWrite(mRd, LOW);
-    }
-    fade( mRa, RlastD, LOW, RlastA, sR);
-    RlastD = LOW;
-    RlastA = sR;
-  }
-  if (ROutput < 0.0) {
-    sR = max(0, 255 - RMotor_offset_bck + ROutput);
-    if (RlastD == LOW){
-      fade( mRa, RlastD, HIGH, RlastA, 127);
-      digitalWrite(mRd, HIGH);
-    }
-    fade( mRa, RlastD, HIGH, RlastA, sR);
-    RlastD = HIGH;
-    RlastA = sR;
-  }
-}
-
-void fade(int motor, int oD, int nD, int oA, int nA) {
-  analogWrite(motor, nA);
-}
-
-void fadeX(int motor, int oD, int nD, int oA, int nA) {
-  int wait = 30;
-  if (nA == 0) {
-    // stop motor
-    if (oD == HIGH) {
-      // fade tp A==D==High==255
-      for (int i = oA; i <= 255; i++) {
-        analogWrite(motor, i);
-        delay(wait);
-      }
-    } else {
-      // fade to A==D==Low==0
-      for (int i = oA; i >= 0; i--) {
-        analogWrite(motor, i);
-        delay(wait);
-      }
-    }
-  }
-  if (nA > 0) {
-    // move forward
-    if (oD == HIGH) {
-      // fade to no motion as last driection was reverse
-      for (int i = oA; i <= 255; i++) {
-        analogWrite(motor, i);
-        delay(wait);
-      }
-      digitalWrite(motor, LOW);
-      for (int i = 0; i <= nA; i++) {
-        analogWrite(motor, i);
-        delay(wait);
-      }
-    } else {
-      if (oA > nA) {
-        for (int i = oA; i >= nA; i--) {
-          analogWrite(motor, i);
-          delay(wait);
-        }
-      } else {
-        for (int i = oA; i <= nA; i++) {
-          analogWrite(motor, i);
-          delay(wait);
-        }
-      }
-    }
-  }
-  if (nA < 0) {
-    // move reverse
-    if (oD == LOW) {
-      // fade to no motion as last driection was forward
-      for (int i = oA; i >= 0; i--) {
-        analogWrite(motor, i);
-        delay(wait);
-      }
-      digitalWrite(motor, HIGH);
-      for (int i = 255; i >= nA; i--) {
-        analogWrite(motor, i);
-        delay(wait);
-      }
-    } else {
-      if (oA > nA) {
-        for (int i = oA; i >= nA; i--) {
-          analogWrite(motor, i);
-          delay(wait);
-        }
-      } else {
-        for (int i = oA; i <= nA; i++) {
-          analogWrite(motor, i);
-          delay(wait);
-        }
-      }
-    }
-  }
-}
+#define DEBUG 1
+
+#include "I2Cdev.h"
+#include "MPU6050.h"
+#include "Wire.h"
+#define mRd 7
+#define mRa 6
+#define mLd 4
+#define mLa 5
+#define Gry_offset 0  //The offset of the gyro
+#define Gyr_Gain 8.000 // 16.348
+
+#define Angle_offset 0  // The offset of the accelerator
+float Angle_Delta, Angle_Recursive, Angle_Confidence;
+
+#define RMotor_offset_fwd 0 // The offset of the Motor
+#define LMotor_offset_fwd 0  // The offset of the Motor
+#define RMotor_offset_bck 0 // The offset of the Motor
+#define LMotor_offset_bck 0  // The offset of the Motor
+#define Mramp 3
+#define pi 3.14159
+float kp, ki, kd;
+float Angle_Raw, Angle_Filtered, omega;
+float Turn_Speed = 0, Turn_Speed_K = 0;
+float Run_Speed = 0, Run_Speed_K = 0;
+float LOutput, ROutput;
+float LlastA, RlastA;
+int LlastD, RlastD;
+unsigned long preTime = 0;
+unsigned long lastTime;
+float Input, Output;
+float errSum, dErr, error, lastErr;
+int timeChange;
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for InvenSense evaluation board)
+// AD0 high = 0x69
+MPU6050 accelgyro;
+MPU6050 initialize;
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+
+void setup() {
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+  Wire.begin();
+#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+  Fastwire::setup(400, true);
+#endif
+  Serial.begin(115200);
+  Serial.println("Initializing I2C devices...");
+  accelgyro.initialize();
+  Serial.println("Testing device connections...");
+  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+  initialize.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);;
+  for (int i = 0; i < 100; i++) // Looping 200 times to get the real gesture when starting
+  {
+    Filter();
+  }
+
+  pinMode(mRd, OUTPUT);
+  pinMode(mRa, OUTPUT);
+  pinMode(mLd, OUTPUT);
+  pinMode(mLa, OUTPUT);
+  LlastA = 0.0;
+  RlastA = 0.0;
+  LlastD = LOW;
+  RlastD = LOW;
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  Filter();
+#ifdef DEBUG
+//  Serial.print(" Raw =\t");
+//  Serial.print(Angle_Raw);
+  Serial.print(" Angle =\t");
+  Serial.print(Angle_Filtered);
+#endif
+  // If angle > 45 or < -45 then stop the robot
+  if (abs(Angle_Filtered) < 45)
+  {
+    // optional position to check external input
+    myPID();
+    PWMControl();
+  }
+  else
+  {
+    // stop wheels
+    digitalWrite(mRd, LOW);
+    analogWrite(mRa, 0);
+    digitalWrite(mLd, LOW);
+    analogWrite(mLa, 0);
+    for (int i = 0; i < 100; i++) // Keep reading the gesture after falling down
+    {
+      Filter();
+    }
+    if (abs(Angle_Filtered) < 45) // Empty datas and restart the robot automaticly
+    {
+      for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds
+      {
+        omega = Angle_Raw = Angle_Filtered = 0;
+        Filter();
+        Output = error = errSum = dErr = Run_Speed = Turn_Speed = 0;
+        myPID();
+      }
+    }
+  }
+#ifdef DEBUG
+  Serial.println();
+  delay(DEBUG);
+#endif
+}
+
+void Filter()
+{
+  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+  //Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset);
+  Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset);
+  omega =  -gx / Gyr_Gain + Gry_offset;
+  // Filters datas to get the real gesture
+  unsigned long now = micros();
+  float dt = (now - preTime) * 0.000001;
+  preTime = now;
+  float K = 0.8;
+  float A = K / (K + dt);
+  Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64;
+  Angle_Recursive = Angle_Delta * dt + Angle_Recursive;
+  Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega;
+  Angle_Filtered = Angle_Confidence * dt + Angle_Filtered;
+  // Preparing datas which need to be shown on the LCD.
+}
+
+void myPID()
+{
+  kp = 4.150; // 22.000
+  ki = 0;
+  kd = 0.00; // 1.60
+  // Calculating the output values using the gesture values and the PID values.
+  error = Angle_Filtered;
+  errSum += error;
+  dErr = error - lastErr;
+  Output = kp * error + ki * errSum + kd * omega;
+  lastErr = error;
+#ifdef DEBUG
+//  Serial.print("\t error:\t");
+ // Serial.print(error);
+//  Serial.print("\t errSum:\t");
+//  Serial.print(errSum);
+//  Serial.print("\t dErr:\t");
+//  Serial.print(dErr);
+//  Serial.print("\t omega:\t");
+//  Serial.print(omega);
+  Serial.print("\t output:\t");
+  Serial.print(Output);
+//  Output = 0;
+#endif
+  LOutput = Output - Turn_Speed + Run_Speed;
+  ROutput = Output + Turn_Speed + Run_Speed;
+}
+
+void PWMControl()
+{
+  // forward min 15 both
+  // reverse min 35 both
+  int sL, sR;
+
+  if (LOutput >= 0.0) {
+    // move forward
+    sL = min(255, LMotor_offset_fwd + LOutput);
+    if (LlastD == HIGH){
+      fade( mLa, LlastD, LOW, LlastA, 127);
+      digitalWrite(mLd, LOW);
+    }
+    fade( mLa, LlastD, LOW, LlastA, sL);
+    LlastD = LOW;
+    LlastA = sL;
+  }
+  if (LOutput < 0.0) {
+    sL = max(0, 255 - LMotor_offset_bck + LOutput);
+    if (LlastD == LOW){
+      fade( mLa, LlastD, HIGH, LlastA, 127);
+      digitalWrite(mLd, HIGH);
+    }
+    fade( mLa, LlastD, HIGH, LlastA, sL);
+    LlastD = HIGH;
+    LlastA = sL;
+  }
+  if (ROutput >= 0.0) {
+    sR = min(255, RMotor_offset_fwd + ROutput);
+    if (RlastD == HIGH){
+      fade( mRa, RlastD, LOW, RlastA, 127);
+      digitalWrite(mRd, LOW);
+    }
+    fade( mRa, RlastD, LOW, RlastA, sR);
+    RlastD = LOW;
+    RlastA = sR;
+  }
+  if (ROutput < 0.0) {
+    sR = max(0, 255 - RMotor_offset_bck + ROutput);
+    if (RlastD == LOW){
+      fade( mRa, RlastD, HIGH, RlastA, 127);
+      digitalWrite(mRd, HIGH);
+    }
+    fade( mRa, RlastD, HIGH, RlastA, sR);
+    RlastD = HIGH;
+    RlastA = sR;
+  }
+}
+
+void fade(int motor, int oD, int nD, int oA, int nA) {
+  analogWrite(motor, nA);
+}
+
+void fadeX(int motor, int oD, int nD, int oA, int nA) {
+  int wait = 30;
+  if (nA == 0) {
+    // stop motor
+    if (oD == HIGH) {
+      // fade tp A==D==High==255
+      for (int i = oA; i <= 255; i++) {
+        analogWrite(motor, i);
+        delay(wait);
+      }
+    } else {
+      // fade to A==D==Low==0
+      for (int i = oA; i >= 0; i--) {
+        analogWrite(motor, i);
+        delay(wait);
+      }
+    }
+  }
+  if (nA > 0) {
+    // move forward
+    if (oD == HIGH) {
+      // fade to no motion as last driection was reverse
+      for (int i = oA; i <= 255; i++) {
+        analogWrite(motor, i);
+        delay(wait);
+      }
+      digitalWrite(motor, LOW);
+      for (int i = 0; i <= nA; i++) {
+        analogWrite(motor, i);
+        delay(wait);
+      }
+    } else {
+      if (oA > nA) {
+        for (int i = oA; i >= nA; i--) {
+          analogWrite(motor, i);
+          delay(wait);
+        }
+      } else {
+        for (int i = oA; i <= nA; i++) {
+          analogWrite(motor, i);
+          delay(wait);
+        }
+      }
+    }
+  }
+  if (nA < 0) {
+    // move reverse
+    if (oD == LOW) {
+      // fade to no motion as last driection was forward
+      for (int i = oA; i >= 0; i--) {
+        analogWrite(motor, i);
+        delay(wait);
+      }
+      digitalWrite(motor, HIGH);
+      for (int i = 255; i >= nA; i--) {
+        analogWrite(motor, i);
+        delay(wait);
+      }
+    } else {
+      if (oA > nA) {
+        for (int i = oA; i >= nA; i--) {
+          analogWrite(motor, i);
+          delay(wait);
+        }
+      } else {
+        for (int i = oA; i <= nA; i++) {
+          analogWrite(motor, i);
+          delay(wait);
+        }
+      }
+    }
+  }
+}