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:
- 5:477eaa33eff5
- Parent:
- 4:a041b7b5720b
- Child:
- 6:af164f09f963
diff -r a041b7b5720b -r 477eaa33eff5 main.cpp --- 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