Diff: main.cpp

Revision:

8:810396d34da8

Parent:

7:337faf16a8d6

Child:

9:59add4a4372c

--- a/main.cpp	Mon Jan 16 05:31:33 2017 +0000
+++ b/main.cpp	Wed Jan 25 19:33:27 2017 +0000
@@ -278,15 +278,15 @@
         BMI160::SensorTime sensorTime;
         
         //Complementary Filter vars, filter weight K
-        static const float K = 0.999F;
+        static const float K = 0.9965F;
         float pitch = 0.0F;
         
         //PID coefficients
 
         static const float DT = 0.00125F;
-        static const float KP = 3.0F;
-        static const float KI = (30.0F*DT);
-        static const float KD = (0.06F/DT);
+        static const float KP = 1.85F;
+        static const float KI = (20.0F*DT);
+        static const float KD = (0.050F/DT);
         float setPoint = 0.0F;
         float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT};
         
@@ -374,7 +374,7 @@
             }
             
             
-            while(start && (pitch > -30.0F) && (pitch < 30.0F))
+            while(start && (pitch > -20.0F) && (pitch < 20.0F))
             {
                 rLED = LED_OFF;
                 gLED = LED_ON;
@@ -486,10 +486,11 @@
                 }
             }
             
-            if((pitch > 30.0F) || (pitch < -30.0F))
+            if((pitch > 20.0F) || (pitch < -20.0F))
             {
                 start = false;
             }
+            
             pitch = 0.0F;
             integral = 0.0F;
             previousError = 0.0F;
@@ -534,6 +535,7 @@
     fp = fopen("/sd/balBot.txt", "a");
     if(fp != NULL)
     {
+        fprintf(fp, "Samples,%d,,,,,\n", numSamples);
         fprintf(fp, "Setpoint,%5.2f,,,,,\n", loopCoeffs[0]);
         fprintf(fp, "K, %f,,,,,\n", loopCoeffs[1]);
         fprintf(fp, "KP, %f,,,,,\n", loopCoeffs[2]);
@@ -576,5 +578,4 @@
 float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT)
 {
     return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
-    //return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * accY));
 }