Diff: main.cpp

Revision:

1:e9dd53326ba1

Parent:

0:f7e385400dec

Child:

2:5af1d818331f

--- a/main.cpp	Fri Jan 06 03:16:04 2017 +0000
+++ b/main.cpp	Fri Jan 06 20:27:17 2017 +0000
@@ -222,17 +222,17 @@
         BMI160::SensorTime sensorTime;
         
         //Complementary Filter vars, filter weight K
-        static const float K = .996F;
+        static const float K = 0.9993F;
         float thetaGyro = 0.0F;
         float thetaAcc = 0.0F;
         float pitch = 0.0F;
         
         //PID coefficients
         static const float DT = 0.000625F;
-        static const float KP = 2.0F;
-        static const float KI = (2.0F * DT);
-        static const float KD = (0.025F / DT);
-        static const float SP = 1.8F;
+        static const float KP = 4.25F;
+        static const float KI = (8.0F * DT);
+        static const float KD = (0.04F / DT);
+        float setPoint = 0.0F;
         
         //Control loop vars
         float currentError, previousError;
@@ -246,22 +246,72 @@
         imu.writeRegister(BMI160::INT_OUT_CTRL, 0x0A); 
         //Map data ready interrupt to INT1
         imu.writeRegister(BMI160::INT_MAP_1, 0x80); 
-        //Tie input to callback fx
+        //Tie INT1 to callback fx
         imuIntIn.rise(&imuISR);
         
+        //Tie SW2 to callback fx
         startStop.fall(&startStopISR);
         
         //control loop timming indicator
         DigitalOut loopPulse(P5_3, 0);
         
+        //Count for averaging setPoint offset, 2 seconds of data
+        uint32_t offsetCount = 0;
+        
         while(1)
         {
             rLED = LED_ON;
-            gLED = LED_OFF;
+            gLED = LED_ON;
+            pitch = 0.0F;
+            setPoint = 0.0F;
             rightPwm.pulsewidth_us(0);
             leftPwm.pulsewidth_us(0);
+            start = false;
+            drdy = false;
             
-            while(start && (pitch > -45.0F) && (pitch < 45.0F))
+            while(offsetCount < 3200)
+            {
+                if(drdy)
+                {
+                    //Clear data ready flag
+                    drdy = false;
+                    
+                    //Read feedback sensors
+                    imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
+                    
+                    //Feedback Block, Complementary filter
+                    //Prediction estimate
+                    thetaGyro = (pitch + (gyroData.xAxis.scaled * DT));
+                    //Measurement estimate
+                    thetaAcc = ((180.0F/3.14F) * atan(accData.yAxis.scaled/accData.zAxis.scaled));
+                    //Feedback, Pitch estimate in degrees
+                    pitch = ((K * thetaGyro) + ((1.0F - K) * thetaAcc));
+                    
+                    //Accumalate pitch measurements
+                    setPoint += pitch;
+                    offsetCount++;
+                }
+            }
+            
+            //Clear count for next time
+            offsetCount = 0;
+            
+            //Average measurements
+            setPoint = setPoint/3200.0F;
+            
+            printf("setPoint = %5.2f\n\n", setPoint);
+            
+            while(!start)
+            {
+                rLED = LED_ON;
+                gLED = LED_ON;
+                wait(0.25);
+                rLED = LED_OFF;
+                gLED = LED_OFF;
+                wait(0.25);
+            }
+            
+            while(start && (pitch > -30.0F) && (pitch < 30.0F))
             {
                 rLED = LED_OFF;
                 gLED = LED_ON;
@@ -287,7 +337,7 @@
                     
                     //PID Block
                     //Error signal
-                    currentError = (SP - pitch);
+                    currentError = (setPoint - pitch);
                     
                     //Integral term, dt is included in KI
                     //If statement addresses integral windup
@@ -315,7 +365,7 @@
                     //printf("Pulsewidth = %f\n\n", pulseWidth);
                     
                     //Clamp to maximum duty cycle and check for forward/reverse 
-                    //based on sign of error signal (negation of pitch since SP = 0)
+                    //based on sign of error signal (negation of pitch since setPoint = 0)
                     if(pulseWidth > MAX_PULSEWIDTH_US)
                     {
                         rightDir = FORWARD;
@@ -356,9 +406,6 @@
                     loopPulse = !loopPulse;
                 }
             }
-            
-            start = false;
-            pitch = 0.0;
         }
     }
     else