Diff: main.cpp

Revision:

5:477eaa33eff5

Parent:

4:a041b7b5720b

Child:

6:af164f09f963

--- a/main.cpp	Tue Jul 21 08:17:56 2015 +0000
+++ b/main.cpp	Wed Jul 22 13:16:04 2015 +0000
@@ -1,7 +1,7 @@
 /*   Brushless gimbal controller with IMU (MPU050)
 *
 *    @author: Baser Kandehir 
-*    @date: July 17, 2015
+*    @date: July 22, 2015
 *    @license: MIT license
 *     
 *   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
@@ -27,10 +27,7 @@
 *    @description of the program: 
 *    
 *    This a one axis gimbal control program that takes roll angle from an IMU 
-*    and  moves the gimbal brushless motor accordingly. Program is written with
-*    PID algorithm but it uses only P for the time being and response is good
-*    enough. Complete PID algorithm can be written by modifying gimbalPID function
-*    and PID constants. 
+*    and moves the gimbal brushless motor accordingly using PID algorithm. 
 *
 *    Microcontroller: LPC1768  
 *    IMU: MPU6050
@@ -49,22 +46,25 @@
 Ticker toggler1;               // Ticker for led toggling
 Ticker filter;                 // Ticker for periodic call to compFilter funcçs 
 Ticker gimbal;                 // Periodic routine for PID control of gimbal system
+Ticker speed;                  // Periodic routine for speed control
 
 /* Function prototypes */
 void toggle_led1();
 void toggle_led2();
 void compFilter();
 void gimbalPID();
+void speedControl();
 
+/* Variable declarations */
 float pitchAngle = 0;
 float rollAngle = 0;
-
-/* Variables to be used in gimbalPID funct. */
-float Kp = 1;
-float Ki = 0; 
-float Kd = 0; 
-float set_point = -45;         // which angle camera should stay
-float errorMargin = 2;         // error margin in degress
+bool dir;           // direction of movement
+bool stop;          // to stop the motor
+float delay;        // time delay between steps
+float Kp = 10;
+float Ki = 0.001; 
+float Kd = 0;
+float set_point = 0;         // which angle camera should stay
 float proportional = 0;
 float last_proportional =0;
 float integral = 0;
@@ -80,17 +80,14 @@
     pc.printf("Calibration is completed. \r\n"); 
     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.005);       // Same period with gimbalPID (important)  
     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);
+     /*   pc.printf("%.1f,%.1f\r\n",rollAngle,pitchAngle);
+        wait_ms(40);*/
     }    
 }
 
@@ -102,8 +99,6 @@
 
 void gimbalPID()
 {
-    bool dir;             // direction of movement
- 
     proportional = set_point - rollAngle;        
     integral += proportional; 
     derivative = proportional - last_proportional;
@@ -111,7 +106,26 @@
     
     errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); 
     (errorPID > 0)?(dir = 1):(dir = 0);
+    
+    /* errorPID is restricted between -400 and 400 */ 
+    if(errorPID > 400)
+        errorPID = 400;
+    else if(errorPID < -400)
+        errorPID = -400;   
    
-    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
+    stop = 0;   
+    delay = 0.1/abs(errorPID);      /* speed should be proportional to error, therefore time delay
+                                     between steps should be inverse proportional to error.*/
+                                      
+    if (abs(errorPID)< Kp/2) stop = 1;  // 0.5 deg noise margin
+         
+    if(stop)  // if the gimbal is within noise margin, dont move.
+        speed.detach();
+    else    
+        speed.attach(&speedControl, delay);      
 }
+
+void speedControl() 
+{    
+    brushlessControl(dir, 0);    
+}
\ No newline at end of file