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@7:b65164847018, 2015-08-05 (annotated)

Committer:
BaserK
Date:
Wed Aug 05 12:49:18 2015 +0000
Revision:
7:b65164847018
Parent:
6:af164f09f963 
i2c object is not extern anymore

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 5:477eaa33eff5 4 * @date: July 22, 2015
BaserK 4:a041b7b5720b 5 * @license: MIT license
BaserK 4:a041b7b5720b 6 *
BaserK 4:a041b7b5720b 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 4:a041b7b5720b 8 *
BaserK 4:a041b7b5720b 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 4:a041b7b5720b 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 4:a041b7b5720b 11 * in the Software without restriction, including without limitation the rights
BaserK 4:a041b7b5720b 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 4:a041b7b5720b 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 4:a041b7b5720b 14 * furnished to do so, subject to the following conditions:
BaserK 4:a041b7b5720b 15 *
BaserK 4:a041b7b5720b 16 * The above copyright notice and this permission notice shall be included in
BaserK 4:a041b7b5720b 17 * all copies or substantial portions of the Software.
BaserK 4:a041b7b5720b 18 *
BaserK 4:a041b7b5720b 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 4:a041b7b5720b 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 4:a041b7b5720b 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 4:a041b7b5720b 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 4:a041b7b5720b 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 4:a041b7b5720b 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 4:a041b7b5720b 25 * THE SOFTWARE.
BaserK 4:a041b7b5720b 26 *
BaserK 2:f9f4d36c2367 27 * @description of the program:
BaserK 2:f9f4d36c2367 28 *
BaserK 2:f9f4d36c2367 29 * This a one axis gimbal control program that takes roll angle from an IMU
BaserK 5:477eaa33eff5 30 * and moves the gimbal brushless motor accordingly using PID algorithm.
BaserK 2:f9f4d36c2367 31 *
BaserK 2:f9f4d36c2367 32 * Microcontroller: LPC1768
BaserK 2:f9f4d36c2367 33 * IMU: MPU6050
BaserK 2:f9f4d36c2367 34 * Motor driver: 2x TB6612FNG
BaserK 2:f9f4d36c2367 35 *
BaserK 2:f9f4d36c2367 36 * Note: For any mistakes or comments, please contact me.
BaserK 2:f9f4d36c2367 37 */
BaserK 2:f9f4d36c2367 38
BaserK 0:40b56bdec1d2 39 #include "mbed.h"
BaserK 0:40b56bdec1d2 40 #include "MPU6050.h"
BaserK 0:40b56bdec1d2 41 #include "ledControl.h"
BaserK 1:2ae94169eee6 42 #include "brushlessController_TB6612FNG.h"
BaserK 0:40b56bdec1d2 43
BaserK 1:2ae94169eee6 44 Serial pc(USBTX, USBRX); // Create terminal link
BaserK 1:2ae94169eee6 45 MPU6050 mpu6050; // mpu6050 object from MPU6050 classs
BaserK 1:2ae94169eee6 46 Ticker toggler1; // Ticker for led toggling
BaserK 1:2ae94169eee6 47 Ticker filter; // Ticker for periodic call to compFilter funcçs
BaserK 2:f9f4d36c2367 48 Ticker gimbal; // Periodic routine for PID control of gimbal system
BaserK 5:477eaa33eff5 49 Ticker speed; // Periodic routine for speed control
BaserK 0:40b56bdec1d2 50
BaserK 1:2ae94169eee6 51 /* Function prototypes */
BaserK 0:40b56bdec1d2 52 void toggle_led1();
BaserK 0:40b56bdec1d2 53 void toggle_led2();
BaserK 0:40b56bdec1d2 54 void compFilter();
BaserK 2:f9f4d36c2367 55 void gimbalPID();
BaserK 5:477eaa33eff5 56 void speedControl();
BaserK 0:40b56bdec1d2 57
BaserK 5:477eaa33eff5 58 /* Variable declarations */
BaserK 0:40b56bdec1d2 59 float pitchAngle = 0;
BaserK 0:40b56bdec1d2 60 float rollAngle = 0;
BaserK 5:477eaa33eff5 61 bool dir; // direction of movement
BaserK 5:477eaa33eff5 62 bool stop; // to stop the motor
BaserK 5:477eaa33eff5 63 float delay; // time delay between steps
BaserK 5:477eaa33eff5 64 float Kp = 10;
BaserK 6:af164f09f963 65 float Ki = 0.0001;
BaserK 6:af164f09f963 66 float Kd = 5;
BaserK 5:477eaa33eff5 67 float set_point = 0; // which angle camera should stay
BaserK 2:f9f4d36c2367 68 float proportional = 0;
BaserK 2:f9f4d36c2367 69 float last_proportional =0;
BaserK 2:f9f4d36c2367 70 float integral = 0;
BaserK 2:f9f4d36c2367 71 float derivative = 0;
BaserK 2:f9f4d36c2367 72 float errorPID = 0; // error is already declared at mbed libraries
BaserK 0:40b56bdec1d2 73
BaserK 1:2ae94169eee6 74 int main()
BaserK 0:40b56bdec1d2 75 {
BaserK 1:2ae94169eee6 76 pc.baud(9600); // baud rate: 9600
BaserK 1:2ae94169eee6 77 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 1:2ae94169eee6 78 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 1:2ae94169eee6 79 pc.printf("Calibration is completed. \r\n");
BaserK 1:2ae94169eee6 80 mpu6050.init(); // Initialize the sensor
BaserK 1:2ae94169eee6 81 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 5:477eaa33eff5 82
BaserK 3:065a064b3453 83 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 5:477eaa33eff5 84 gimbal.attach(&gimbalPID, 0.005); // Same period with gimbalPID (important)
BaserK 1:2ae94169eee6 85 while(1)
BaserK 6:af164f09f963 86 {
BaserK 7:b65164847018 87 pc.printf("%.1f,%.1f\r\n",rollAngle,pitchAngle);
BaserK 7:b65164847018 88 wait_ms(40);
BaserK 1:2ae94169eee6 89 }
BaserK 0:40b56bdec1d2 90 }
BaserK 0:40b56bdec1d2 91
BaserK 0:40b56bdec1d2 92 void toggle_led1() {ledToggle(1);}
BaserK 0:40b56bdec1d2 93 void toggle_led2() {ledToggle(2);}
BaserK 0:40b56bdec1d2 94
BaserK 0:40b56bdec1d2 95 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 2:f9f4d36c2367 96 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 2:f9f4d36c2367 97
BaserK 2:f9f4d36c2367 98 void gimbalPID()
BaserK 2:f9f4d36c2367 99 {
BaserK 2:f9f4d36c2367 100 proportional = set_point - rollAngle;
BaserK 2:f9f4d36c2367 101 integral += proportional;
BaserK 2:f9f4d36c2367 102 derivative = proportional - last_proportional;
BaserK 2:f9f4d36c2367 103 last_proportional = proportional;
BaserK 2:f9f4d36c2367 104
BaserK 2:f9f4d36c2367 105 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
BaserK 2:f9f4d36c2367 106 (errorPID > 0)?(dir = 1):(dir = 0);
BaserK 5:477eaa33eff5 107
BaserK 5:477eaa33eff5 108 /* errorPID is restricted between -400 and 400 */
BaserK 5:477eaa33eff5 109 if(errorPID > 400)
BaserK 5:477eaa33eff5 110 errorPID = 400;
BaserK 5:477eaa33eff5 111 else if(errorPID < -400)
BaserK 5:477eaa33eff5 112 errorPID = -400;
BaserK 3:065a064b3453 113
BaserK 5:477eaa33eff5 114 stop = 0;
BaserK 5:477eaa33eff5 115 delay = 0.1/abs(errorPID); /* speed should be proportional to error, therefore time delay
BaserK 5:477eaa33eff5 116 between steps should be inverse proportional to error.*/
BaserK 5:477eaa33eff5 117
BaserK 5:477eaa33eff5 118 if (abs(errorPID)< Kp/2) stop = 1; // 0.5 deg noise margin
BaserK 5:477eaa33eff5 119
BaserK 5:477eaa33eff5 120 if(stop) // if the gimbal is within noise margin, dont move.
BaserK 5:477eaa33eff5 121 speed.detach();
BaserK 5:477eaa33eff5 122 else
BaserK 5:477eaa33eff5 123 speed.attach(&speedControl, delay);
BaserK 2:f9f4d36c2367 124 }
BaserK 5:477eaa33eff5 125
BaserK 5:477eaa33eff5 126 void speedControl()
BaserK 5:477eaa33eff5 127 {
BaserK 5:477eaa33eff5 128 brushlessControl(dir, 0);
BaserK 5:477eaa33eff5 129 }