This is a one axis gimbal control program that takes roll angle from an IMU and moves the gimbal brushless motor accordingly.
Dependencies: MPU6050 brushlessController_TB6612FNG ledControl2 mbed
Diff: main.cpp
- Revision:
- 2:f9f4d36c2367
- Parent:
- 1:2ae94169eee6
- Child:
- 3:065a064b3453
diff -r 2ae94169eee6 -r f9f4d36c2367 main.cpp --- 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 +}