Diff: main.cpp

Revision:

11:371026db1dcd

Parent:

10:71c31197d55a

Child:

12:0fb3a03aeecb

--- a/main.cpp	Wed Jan 25 19:44:43 2017 +0000
+++ b/main.cpp	Sat Jan 28 05:10:07 2017 +0000
@@ -278,15 +278,17 @@
         BMI160::SensorTime sensorTime;
         
         //Complementary Filter vars, filter weight K
-        static const float K = 0.9965F;
+        static const float K = 0.9978F;
         float pitch = 0.0F;
         
         //PID coefficients
+        
+        //tunning with Wilson, Kc = 4.2, pc = 0.166
 
         static const float DT = 0.00125F;
-        static const float KP = 1.85F;
-        static const float KI = (20.0F*DT);
-        static const float KD = (0.050F/DT);
+        static const float KP = 2.5F; 
+        static const float KI = (30.0F*DT); 
+        static const float KD = (0.05F/DT);
         float setPoint = 0.0F;
         float loopCoeffs[] = {setPoint, K, KP, KI, KD, DT};
         
@@ -324,9 +326,9 @@
         pingTriggerTimer.attach(&triggerPing, 0.05);
         
         //Position control vars/constants
-        static const float PING_SP = 20.0F;
-        static const float PING_KP = 0.0F;
-        float pingCurrentError = 0.0F;
+        //static const float PING_SP = 20.0F;
+        //static const float PING_KP = 0.0F;
+        //float pingCurrentError = 0.0F;
         
         //control loop timming indicator
         DigitalOut loopPulse(P5_3, 0);
@@ -398,31 +400,17 @@
                     //Error signal
                     currentError = (setPoint - pitch);
                     
-                    //Integral term, dt is included in KI
-                    //If statement addresses integral windup
-                    if(currentError == 0.0F)
-                    {
-                        integral = 0.0F;
-                    }
-                    else if(((currentError < 0.0F) && (previousError > 0.0F)) || 
-                           ((currentError > 0.0F) && (previousError < 0.0F)))
-                    {
-                        integral = 0.0F;
-                    }
-                    else
-                    {
-                        integral = (integral + currentError);
-                    }
+                    integral = (integral + currentError);
                     
                     //Derivative term, dt is included in KD
                     derivative = (currentError - previousError);
                     
-                    
                     //Set right/left pulsewidth
                     //Just balance for now, so both left/right are the same
                     pulseWidth = ((KP * currentError) + (KI * integral) + (KD * derivative));
                     
                     
+                    /*
                     if(pingReady)
                     {
                         //Get error signal
@@ -431,6 +419,7 @@
                     }
                     
                     pulseWidth += (pingCurrentError * PING_KP);
+                    */
                     
                     
                     //Clamp to maximum duty cycle and check for forward/reverse