Baser Kandehir / Mbed 2 deprecated gimbalController_brushless_IMU Featured

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

main.cpp@3:065a064b3453, 2015-07-21 (annotated)

Committer:
BaserK
Date:
Tue Jul 21 06:35:34 2015 +0000
Revision:
3:065a064b3453
Parent:
2:f9f4d36c2367
Child:
4:a041b7b5720b
Coarse and fine tuning added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 2:f9f4d36c2367 1 /* Brushless gimbal controller with IMU (MPU050)
BaserK 2:f9f4d36c2367 2 *
BaserK 2:f9f4d36c2367 3 * @author: Baser Kandehir
BaserK 2:f9f4d36c2367 4 * @date: July 17, 2015
BaserK 2:f9f4d36c2367 5 * @license: Use this code however you'd like
BaserK 2:f9f4d36c2367 6 *
BaserK 2:f9f4d36c2367 7 * @description of the program:
BaserK 2:f9f4d36c2367 8 *
BaserK 2:f9f4d36c2367 9 * This a one axis gimbal control program that takes roll angle from an IMU
BaserK 2:f9f4d36c2367 10 * and moves the gimbal brushless motor accordingly. Program is written with
BaserK 2:f9f4d36c2367 11 * PID algorithm but it uses only P for the time being and response is good
BaserK 2:f9f4d36c2367 12 * enough. Complete PID algorithm can be written by modifying gimbalPID function
BaserK 2:f9f4d36c2367 13 * and PID constants.
BaserK 2:f9f4d36c2367 14 *
BaserK 2:f9f4d36c2367 15 * Microcontroller: LPC1768
BaserK 2:f9f4d36c2367 16 * IMU: MPU6050
BaserK 2:f9f4d36c2367 17 * Motor driver: 2x TB6612FNG
BaserK 2:f9f4d36c2367 18 *
BaserK 2:f9f4d36c2367 19 * Note: For any mistakes or comments, please contact me.
BaserK 2:f9f4d36c2367 20 */
BaserK 2:f9f4d36c2367 21
BaserK 0:40b56bdec1d2 22 #include "mbed.h"
BaserK 0:40b56bdec1d2 23 #include "MPU6050.h"
BaserK 0:40b56bdec1d2 24 #include "ledControl.h"
BaserK 1:2ae94169eee6 25 #include "brushlessController_TB6612FNG.h"
BaserK 0:40b56bdec1d2 26
BaserK 1:2ae94169eee6 27 Serial pc(USBTX, USBRX); // Create terminal link
BaserK 1:2ae94169eee6 28 MPU6050 mpu6050; // mpu6050 object from MPU6050 classs
BaserK 1:2ae94169eee6 29 Ticker toggler1; // Ticker for led toggling
BaserK 1:2ae94169eee6 30 Ticker filter; // Ticker for periodic call to compFilter funcçs
BaserK 2:f9f4d36c2367 31 Ticker gimbal; // Periodic routine for PID control of gimbal system
BaserK 0:40b56bdec1d2 32
BaserK 1:2ae94169eee6 33 /* Function prototypes */
BaserK 0:40b56bdec1d2 34 void toggle_led1();
BaserK 0:40b56bdec1d2 35 void toggle_led2();
BaserK 0:40b56bdec1d2 36 void compFilter();
BaserK 2:f9f4d36c2367 37 void gimbalPID();
BaserK 0:40b56bdec1d2 38
BaserK 0:40b56bdec1d2 39 float pitchAngle = 0;
BaserK 0:40b56bdec1d2 40 float rollAngle = 0;
BaserK 2:f9f4d36c2367 41
BaserK 2:f9f4d36c2367 42 /* Variables to be used in gimbalPID funct. */
BaserK 2:f9f4d36c2367 43 float Kp = 1;
BaserK 2:f9f4d36c2367 44 float Ki = 0;
BaserK 2:f9f4d36c2367 45 float Kd = 0;
BaserK 3:065a064b3453 46 float set_point = -90; // which angle camera should stay
BaserK 3:065a064b3453 47 float errorMargin = 2; // error margin in degress
BaserK 2:f9f4d36c2367 48 float proportional = 0;
BaserK 2:f9f4d36c2367 49 float last_proportional =0;
BaserK 2:f9f4d36c2367 50 float integral = 0;
BaserK 2:f9f4d36c2367 51 float derivative = 0;
BaserK 2:f9f4d36c2367 52 float errorPID = 0; // error is already declared at mbed libraries
BaserK 0:40b56bdec1d2 53
BaserK 1:2ae94169eee6 54 int main()
BaserK 0:40b56bdec1d2 55 {
BaserK 1:2ae94169eee6 56 pc.baud(9600); // baud rate: 9600
BaserK 1:2ae94169eee6 57 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 1:2ae94169eee6 58 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 1:2ae94169eee6 59 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 1:2ae94169eee6 60 pc.printf("Calibration is completed. \r\n");
BaserK 1:2ae94169eee6 61 mpu6050.init(); // Initialize the sensor
BaserK 1:2ae94169eee6 62 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:40b56bdec1d2 63
BaserK 3:065a064b3453 64 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 1:2ae94169eee6 65 while(1)
BaserK 2:f9f4d36c2367 66 {
BaserK 3:065a064b3453 67 if(abs(errorPID) < 10)
BaserK 3:065a064b3453 68 gimbal.attach(&gimbalPID, 0.005); // Fine tuning
BaserK 3:065a064b3453 69 else
BaserK 3:065a064b3453 70 gimbal.attach(&gimbalPID, 0.001); // Coarse tuning
BaserK 3:065a064b3453 71
BaserK 3:065a064b3453 72 pc.printf("%.1f,%.1f\r\n",rollAngle,pitchAngle);
BaserK 3:065a064b3453 73 wait_ms(40);
BaserK 1:2ae94169eee6 74 }
BaserK 0:40b56bdec1d2 75 }
BaserK 0:40b56bdec1d2 76
BaserK 0:40b56bdec1d2 77 void toggle_led1() {ledToggle(1);}
BaserK 0:40b56bdec1d2 78 void toggle_led2() {ledToggle(2);}
BaserK 0:40b56bdec1d2 79
BaserK 0:40b56bdec1d2 80 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 2:f9f4d36c2367 81 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 2:f9f4d36c2367 82
BaserK 2:f9f4d36c2367 83 void gimbalPID()
BaserK 2:f9f4d36c2367 84 {
BaserK 3:065a064b3453 85 bool dir; // direction of movement
BaserK 2:f9f4d36c2367 86
BaserK 2:f9f4d36c2367 87 proportional = set_point - rollAngle;
BaserK 2:f9f4d36c2367 88 integral += proportional;
BaserK 2:f9f4d36c2367 89 derivative = proportional - last_proportional;
BaserK 2:f9f4d36c2367 90 last_proportional = proportional;
BaserK 2:f9f4d36c2367 91
BaserK 2:f9f4d36c2367 92 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
BaserK 2:f9f4d36c2367 93 (errorPID > 0)?(dir = 1):(dir = 0);
BaserK 3:065a064b3453 94
BaserK 3:065a064b3453 95 if(abs(errorPID) > errorMargin) // Within error margin motor wont move. This is for stabilizing the gimbal system
BaserK 2:f9f4d36c2367 96 brushlessControl(dir, 0); // No need for a delay time because gimbalPID function is periodic
BaserK 2:f9f4d36c2367 97 }