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@4:a041b7b5720b, 2015-07-21 (annotated)

Committer:
BaserK
Date:
Tue Jul 21 08:17:56 2015 +0000
Revision:
4:a041b7b5720b
Parent:
3:065a064b3453
Child:
5:477eaa33eff5
MIT license is 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 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 2:f9f4d36c2367 30 * and moves the gimbal brushless motor accordingly. Program is written with
BaserK 2:f9f4d36c2367 31 * PID algorithm but it uses only P for the time being and response is good
BaserK 2:f9f4d36c2367 32 * enough. Complete PID algorithm can be written by modifying gimbalPID function
BaserK 2:f9f4d36c2367 33 * and PID constants.
BaserK 2:f9f4d36c2367 34 *
BaserK 2:f9f4d36c2367 35 * Microcontroller: LPC1768
BaserK 2:f9f4d36c2367 36 * IMU: MPU6050
BaserK 2:f9f4d36c2367 37 * Motor driver: 2x TB6612FNG
BaserK 2:f9f4d36c2367 38 *
BaserK 2:f9f4d36c2367 39 * Note: For any mistakes or comments, please contact me.
BaserK 2:f9f4d36c2367 40 */
BaserK 2:f9f4d36c2367 41
BaserK 0:40b56bdec1d2 42 #include "mbed.h"
BaserK 0:40b56bdec1d2 43 #include "MPU6050.h"
BaserK 0:40b56bdec1d2 44 #include "ledControl.h"
BaserK 1:2ae94169eee6 45 #include "brushlessController_TB6612FNG.h"
BaserK 0:40b56bdec1d2 46
BaserK 1:2ae94169eee6 47 Serial pc(USBTX, USBRX); // Create terminal link
BaserK 1:2ae94169eee6 48 MPU6050 mpu6050; // mpu6050 object from MPU6050 classs
BaserK 1:2ae94169eee6 49 Ticker toggler1; // Ticker for led toggling
BaserK 1:2ae94169eee6 50 Ticker filter; // Ticker for periodic call to compFilter funcçs
BaserK 2:f9f4d36c2367 51 Ticker gimbal; // Periodic routine for PID control of gimbal system
BaserK 0:40b56bdec1d2 52
BaserK 1:2ae94169eee6 53 /* Function prototypes */
BaserK 0:40b56bdec1d2 54 void toggle_led1();
BaserK 0:40b56bdec1d2 55 void toggle_led2();
BaserK 0:40b56bdec1d2 56 void compFilter();
BaserK 2:f9f4d36c2367 57 void gimbalPID();
BaserK 0:40b56bdec1d2 58
BaserK 0:40b56bdec1d2 59 float pitchAngle = 0;
BaserK 0:40b56bdec1d2 60 float rollAngle = 0;
BaserK 2:f9f4d36c2367 61
BaserK 2:f9f4d36c2367 62 /* Variables to be used in gimbalPID funct. */
BaserK 2:f9f4d36c2367 63 float Kp = 1;
BaserK 2:f9f4d36c2367 64 float Ki = 0;
BaserK 2:f9f4d36c2367 65 float Kd = 0;
BaserK 4:a041b7b5720b 66 float set_point = -45; // which angle camera should stay
BaserK 3:065a064b3453 67 float errorMargin = 2; // error margin in degress
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 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 1:2ae94169eee6 78 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 1:2ae94169eee6 79 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 1:2ae94169eee6 80 pc.printf("Calibration is completed. \r\n");
BaserK 1:2ae94169eee6 81 mpu6050.init(); // Initialize the sensor
BaserK 1:2ae94169eee6 82 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:40b56bdec1d2 83
BaserK 3:065a064b3453 84 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 1:2ae94169eee6 85 while(1)
BaserK 2:f9f4d36c2367 86 {
BaserK 3:065a064b3453 87 if(abs(errorPID) < 10)
BaserK 3:065a064b3453 88 gimbal.attach(&gimbalPID, 0.005); // Fine tuning
BaserK 3:065a064b3453 89 else
BaserK 3:065a064b3453 90 gimbal.attach(&gimbalPID, 0.001); // Coarse tuning
BaserK 3:065a064b3453 91
BaserK 3:065a064b3453 92 pc.printf("%.1f,%.1f\r\n",rollAngle,pitchAngle);
BaserK 3:065a064b3453 93 wait_ms(40);
BaserK 1:2ae94169eee6 94 }
BaserK 0:40b56bdec1d2 95 }
BaserK 0:40b56bdec1d2 96
BaserK 0:40b56bdec1d2 97 void toggle_led1() {ledToggle(1);}
BaserK 0:40b56bdec1d2 98 void toggle_led2() {ledToggle(2);}
BaserK 0:40b56bdec1d2 99
BaserK 0:40b56bdec1d2 100 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 2:f9f4d36c2367 101 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 2:f9f4d36c2367 102
BaserK 2:f9f4d36c2367 103 void gimbalPID()
BaserK 2:f9f4d36c2367 104 {
BaserK 3:065a064b3453 105 bool dir; // direction of movement
BaserK 2:f9f4d36c2367 106
BaserK 2:f9f4d36c2367 107 proportional = set_point - rollAngle;
BaserK 2:f9f4d36c2367 108 integral += proportional;
BaserK 2:f9f4d36c2367 109 derivative = proportional - last_proportional;
BaserK 2:f9f4d36c2367 110 last_proportional = proportional;
BaserK 2:f9f4d36c2367 111
BaserK 2:f9f4d36c2367 112 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
BaserK 2:f9f4d36c2367 113 (errorPID > 0)?(dir = 1):(dir = 0);
BaserK 3:065a064b3453 114
BaserK 3:065a064b3453 115 if(abs(errorPID) > errorMargin) // Within error margin motor wont move. This is for stabilizing the gimbal system
BaserK 2:f9f4d36c2367 116 brushlessControl(dir, 0); // No need for a delay time because gimbalPID function is periodic
BaserK 2:f9f4d36c2367 117 }