Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

main.cpp@2:f9f4d36c2367, 2015-07-17 (annotated)

Committer:
BaserK
Date:
Fri Jul 17 16:51:54 2015 +0000
Revision:
2:f9f4d36c2367
Parent:
1:2ae94169eee6
Child:
3:065a064b3453
PID algorithm 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 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 2:f9f4d36c2367 46 float set_point = -30; // which angle camera should stay
BaserK 2:f9f4d36c2367 47 float errorMargin = 5; // 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 1:2ae94169eee6 64 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 2:f9f4d36c2367 65 gimbal.attach(&gimbalPID, 0.001); // Call the gimbalPID func. every 1 ms
BaserK 1:2ae94169eee6 66 while(1)
BaserK 2:f9f4d36c2367 67 {
BaserK 1:2ae94169eee6 68 }
BaserK 0:40b56bdec1d2 69 }
BaserK 0:40b56bdec1d2 70
BaserK 0:40b56bdec1d2 71 void toggle_led1() {ledToggle(1);}
BaserK 0:40b56bdec1d2 72 void toggle_led2() {ledToggle(2);}
BaserK 0:40b56bdec1d2 73
BaserK 0:40b56bdec1d2 74 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 2:f9f4d36c2367 75 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 2:f9f4d36c2367 76
BaserK 2:f9f4d36c2367 77 void gimbalPID()
BaserK 2:f9f4d36c2367 78 {
BaserK 2:f9f4d36c2367 79 bool dir; // direction of movement
BaserK 2:f9f4d36c2367 80
BaserK 2:f9f4d36c2367 81 proportional = set_point - rollAngle;
BaserK 2:f9f4d36c2367 82 integral += proportional;
BaserK 2:f9f4d36c2367 83 derivative = proportional - last_proportional;
BaserK 2:f9f4d36c2367 84 last_proportional = proportional;
BaserK 2:f9f4d36c2367 85
BaserK 2:f9f4d36c2367 86 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
BaserK 2:f9f4d36c2367 87 (errorPID > 0)?(dir = 1):(dir = 0);
BaserK 2:f9f4d36c2367 88
BaserK 2:f9f4d36c2367 89 if(abs(errorPID) > errorMargin) // Within error margin motor wont move. This is for stabilizing the gimbal system
BaserK 2:f9f4d36c2367 90 brushlessControl(dir, 0); // No need for a delay time because gimbalPID function is periodic
BaserK 2:f9f4d36c2367 91 }