Diff: main.cpp

Revision:

2:f9f4d36c2367

Parent:

1:2ae94169eee6

Child:

3:065a064b3453

--- a/main.cpp	Fri Jul 17 12:21:14 2015 +0000
+++ b/main.cpp	Fri Jul 17 16:51:54 2015 +0000
@@ -1,3 +1,24 @@
+/*   Brushless gimbal controller with IMU (MPU050)
+*
+*    @author: Baser Kandehir 
+*    @date: July 17, 2015
+*    @license: Use this code however you'd like
+*   
+*    @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. 
+*
+*    Microcontroller: LPC1768  
+*    IMU: MPU6050
+*    Motor driver: 2x TB6612FNG
+*
+*   Note: For any mistakes or comments, please contact me.
+*/
+
 #include "mbed.h"
 #include "MPU6050.h"
 #include "ledControl.h"
@@ -7,16 +28,28 @@
 MPU6050 mpu6050;               // mpu6050 object from MPU6050 classs
 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
 
 /* Function prototypes */
 void toggle_led1();
 void toggle_led2();
 void compFilter();
+void gimbalPID();
 
 float pitchAngle = 0;
 float rollAngle = 0;
-int errorMargin = 8;   // error margin in degrees for stabilizing the gimbal system
-int setPoint = -30;      // set point in degrees for the gimbal system 
+
+/* Variables to be used in gimbalPID funct. */
+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 proportional = 0;
+float last_proportional =0;
+float integral = 0;
+float derivative = 0;
+float errorPID = 0;            // error is already declared at mbed libraries
 
 int main()
 {
@@ -29,23 +62,9 @@
     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
     while(1)
-    {
-        if (abs(rollAngle - setPoint) < errorMargin)
-        {
-            // Do not move if above statement is true
-            led4 = 1;
-        }          
-        else if(rollAngle > setPoint)
-        {
-            brushlessControl(1, 500);  
-            led4 = 0;
-        }
-        else 
-        {   
-            brushlessControl(0, 500);  
-            led4 = 0;
-        }
+    { 
     }    
 }
 
@@ -53,4 +72,20 @@
 void toggle_led2() {ledToggle(2);}
 
 /* This function is created to avoid address error that caused from Ticker.attach func */ 
-void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
\ No newline at end of file
+void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
+
+void gimbalPID()
+{
+    bool dir;  // direction of movement
+ 
+    proportional = set_point - rollAngle;        
+    integral += proportional; 
+    derivative = proportional - last_proportional;
+    last_proportional = proportional;
+    
+    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
+        brushlessControl(dir, 0);    // No need for a delay time because gimbalPID function is periodic
+}