From ae759e172c6067f55a1596f582a67b90f2fd98fd Mon Sep 17 00:00:00 2001
From: Reto <reto@wff.ch>
Date: Sun, 7 Jul 2019 17:01:28 +0200
Subject: [PATCH] Parameter Balance 2

---
 witty.ino | 21 ++++++++++-----------
 1 file changed, 10 insertions(+), 11 deletions(-)

diff --git a/witty.ino b/witty.ino
index c60c4a1..f833241 100644
--- a/witty.ino
+++ b/witty.ino
@@ -11,12 +11,13 @@
 #define Gyr_Gain 8.000 // 16.348
 
 #define Angle_offset 0  // The offset of the accelerator
+#define Angle_to_balance 80 // The offset to the balance angle
 float Angle_Delta, Angle_Recursive, Angle_Confidence;
 
-#define RMotor_offset_fwd 0 // The offset of the Motor
+#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 RMotor_offset_bck 58 // The offset of the Motor
+#define LMotor_offset_bck 58 // The offset of the Motor
 #define Mramp 3
 #define pi 3.14159
 float kp, ki, kd;
@@ -55,7 +56,7 @@ void setup() {
   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
+  for (int i = 0; i < 100; i++) // Looping 100 times to get the real gesture when starting
   {
     Filter();
   }
@@ -80,7 +81,7 @@ void loop() {
   Serial.print(Angle_Filtered);
 #endif
   // If angle > 45 or < -45 then stop the robot
-  if (abs(Angle_Filtered) < 45)
+  if (abs(Angle_Filtered - Angle_to_balance ) < 75)
   {
     // optional position to check external input
     myPID();
@@ -97,9 +98,9 @@ void loop() {
     {
       Filter();
     }
-    if (abs(Angle_Filtered) < 45) // Empty datas and restart the robot automaticly
+    if (abs(Angle_Filtered) < 75) // Empty datas and restart the robot automaticly
     {
-      for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds
+      for (int i = 0; i <= 100; i++) // Reset the robot and delay 2 seconds
       {
         omega = Angle_Raw = Angle_Filtered = 0;
         Filter();
@@ -117,7 +118,6 @@ void loop() {
 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
@@ -130,16 +130,15 @@ void Filter()
   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.110; // 22.000
+  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;
+  error = Angle_Filtered - Angle_to_balance; // move 25°
   errSum += error;
   dErr = error - lastErr;
   Output = kp * error + ki * errSum + kd * omega;