Diff: main.cpp

Revision:

3:065a064b3453

Parent:

2:f9f4d36c2367

Child:

4:a041b7b5720b

--- a/main.cpp	Fri Jul 17 16:51:54 2015 +0000
+++ b/main.cpp	Tue Jul 21 06:35:34 2015 +0000
@@ -43,8 +43,8 @@
 float Kp = 1;
 float Ki = 0; 
 float Kd = 0; 
-float set_point = -30;         // which angle camera should stay
-float errorMargin = 5;         // error margin in degress
+float set_point = -90;         // which angle camera should stay
+float errorMargin = 2;         // error margin in degress
 float proportional = 0;
 float last_proportional =0;
 float integral = 0;
@@ -61,10 +61,16 @@
     mpu6050.init();                              // Initialize the sensor
     pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
     
-    filter.attach(&compFilter, 0.005);           // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
-    gimbal.attach(&gimbalPID,  0.001);           // Call the gimbalPID func. every 1 ms
+    filter.attach(&compFilter, 0.005);       // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)  
     while(1)
     { 
+        if(abs(errorPID) < 10)                  
+            gimbal.attach(&gimbalPID,  0.005);     // Fine tuning
+        else    
+            gimbal.attach(&gimbalPID,  0.001);     // Coarse tuning    
+        
+        pc.printf("%.1f,%.1f\r\n",rollAngle,pitchAngle);
+        wait_ms(40);
     }    
 }
 
@@ -76,7 +82,7 @@
 
 void gimbalPID()
 {
-    bool dir;  // direction of movement
+    bool dir;             // direction of movement
  
     proportional = set_point - rollAngle;        
     integral += proportional; 
@@ -85,7 +91,7 @@
     
     errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); 
     (errorPID > 0)?(dir = 1):(dir = 0);
-    
-    if(abs(errorPID) > errorMargin)  // Within error margin motor wont move. This is for stabilizing the gimbal system
+   
+    if(abs(errorPID) > errorMargin)  // Within error margin motor wont move. This is for stabilizing the gimbal system   
         brushlessControl(dir, 0);    // No need for a delay time because gimbalPID function is periodic
 }