Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

main.cpp@0:40b56bdec1d2, 2015-07-14 (annotated)

Committer:
BaserK
Date:
Tue Jul 14 10:45:54 2015 +0000
Revision:
0:40b56bdec1d2
Child:
1:2ae94169eee6
First working version of the gimbalController. It is a little bit shaking but it is fast enough.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:40b56bdec1d2 1 #include "mbed.h"
BaserK 0:40b56bdec1d2 2 #include "MPU6050.h"
BaserK 0:40b56bdec1d2 3 #include "ledControl.h"
BaserK 0:40b56bdec1d2 4 #include "brushlessController_L293D.h"
BaserK 0:40b56bdec1d2 5
BaserK 0:40b56bdec1d2 6 /* Defined in the MPU6050.cpp file */
BaserK 0:40b56bdec1d2 7 // I2C i2c(p9,p10); // setup i2c (SDA,SCL)
BaserK 0:40b56bdec1d2 8
BaserK 0:40b56bdec1d2 9 Serial ftdi(p13,p14); // default baud rate: 9600
BaserK 0:40b56bdec1d2 10 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
BaserK 0:40b56bdec1d2 11 Ticker toggler1;
BaserK 0:40b56bdec1d2 12 Ticker filter;
BaserK 0:40b56bdec1d2 13
BaserK 0:40b56bdec1d2 14 void toggle_led1();
BaserK 0:40b56bdec1d2 15 void toggle_led2();
BaserK 0:40b56bdec1d2 16 void complementaryFilter(float* pitch, float* roll);
BaserK 0:40b56bdec1d2 17 void compFilter();
BaserK 0:40b56bdec1d2 18
BaserK 0:40b56bdec1d2 19 float pitchAngle = 0;
BaserK 0:40b56bdec1d2 20 float rollAngle = 0;
BaserK 0:40b56bdec1d2 21 int prevStep = 0; // previous step for the brushless motor
BaserK 0:40b56bdec1d2 22 int errorMargin = 6; // error margin in degrees for stabilizing the gimbal system
BaserK 0:40b56bdec1d2 23 int setPoint = 0; // set point in degrees for the gimbal system
BaserK 0:40b56bdec1d2 24
BaserK 0:40b56bdec1d2 25 int main()
BaserK 0:40b56bdec1d2 26 {
BaserK 0:40b56bdec1d2 27 ftdi.baud(9600); // baud rate: 9600
BaserK 0:40b56bdec1d2 28 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 0:40b56bdec1d2 29 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:40b56bdec1d2 30 // wait(1);
BaserK 0:40b56bdec1d2 31 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 0:40b56bdec1d2 32 ftdi.printf("Calibration is completed. \r\n");
BaserK 0:40b56bdec1d2 33 // wait(0.5);
BaserK 0:40b56bdec1d2 34 mpu6050.init(); // Initialize the sensor
BaserK 0:40b56bdec1d2 35 // wait(1);
BaserK 0:40b56bdec1d2 36 ftdi.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:40b56bdec1d2 37 // wait_ms(500);
BaserK 0:40b56bdec1d2 38
BaserK 0:40b56bdec1d2 39 while(1)
BaserK 0:40b56bdec1d2 40 {
BaserK 0:40b56bdec1d2 41 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:40b56bdec1d2 42
BaserK 0:40b56bdec1d2 43 if (abs(rollAngle - setPoint) < errorMargin)
BaserK 0:40b56bdec1d2 44 {
BaserK 0:40b56bdec1d2 45 // Do not move if above statement is true
BaserK 0:40b56bdec1d2 46 led4 = 1;
BaserK 0:40b56bdec1d2 47 }
BaserK 0:40b56bdec1d2 48 else if(rollAngle > setPoint)
BaserK 0:40b56bdec1d2 49 {
BaserK 0:40b56bdec1d2 50 oneStep(0,30, &prevStep);
BaserK 0:40b56bdec1d2 51 led4 = 0;
BaserK 0:40b56bdec1d2 52 }
BaserK 0:40b56bdec1d2 53 else
BaserK 0:40b56bdec1d2 54 {
BaserK 0:40b56bdec1d2 55 oneStep(1,30, &prevStep);
BaserK 0:40b56bdec1d2 56 led4 = 0;
BaserK 0:40b56bdec1d2 57 }
BaserK 0:40b56bdec1d2 58 wait_ms(5); // wait for new rollAngle data to arrive
BaserK 0:40b56bdec1d2 59 }
BaserK 0:40b56bdec1d2 60 }
BaserK 0:40b56bdec1d2 61
BaserK 0:40b56bdec1d2 62 void toggle_led1() {ledToggle(1);}
BaserK 0:40b56bdec1d2 63 void toggle_led2() {ledToggle(2);}
BaserK 0:40b56bdec1d2 64
BaserK 0:40b56bdec1d2 65 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 0:40b56bdec1d2 66 void compFilter() {complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 0:40b56bdec1d2 67
BaserK 0:40b56bdec1d2 68 void complementaryFilter(float* pitch, float* roll)
BaserK 0:40b56bdec1d2 69 {
BaserK 0:40b56bdec1d2 70 /* Get actual acc value */
BaserK 0:40b56bdec1d2 71 mpu6050.readAccelData(accelData);
BaserK 0:40b56bdec1d2 72 mpu6050.getAres();
BaserK 0:40b56bdec1d2 73 ax = accelData[0]*aRes - accelBias[0];
BaserK 0:40b56bdec1d2 74 ay = accelData[1]*aRes - accelBias[1];
BaserK 0:40b56bdec1d2 75 az = accelData[2]*aRes - accelBias[2];
BaserK 0:40b56bdec1d2 76
BaserK 0:40b56bdec1d2 77 /* Get actual gyro value */
BaserK 0:40b56bdec1d2 78 mpu6050.readGyroData(gyroData);
BaserK 0:40b56bdec1d2 79 mpu6050.getGres();
BaserK 0:40b56bdec1d2 80 gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i]
BaserK 0:40b56bdec1d2 81 gy = gyroData[1]*gRes; // - gyroBias[1];
BaserK 0:40b56bdec1d2 82 gz = gyroData[2]*gRes; // - gyroBias[2];
BaserK 0:40b56bdec1d2 83
BaserK 0:40b56bdec1d2 84 float pitchAcc, rollAcc;
BaserK 0:40b56bdec1d2 85
BaserK 0:40b56bdec1d2 86 /* Integrate the gyro data(deg/s) over time to get angle */
BaserK 0:40b56bdec1d2 87 *pitch += gx * dt; // Angle around the X-axis
BaserK 0:40b56bdec1d2 88 *roll -= gy * dt; // Angle around the Y-axis
BaserK 0:40b56bdec1d2 89
BaserK 0:40b56bdec1d2 90 /* Turning around the X-axis results in a vector on the Y-axis
BaserK 0:40b56bdec1d2 91 whereas turning around the Y-axis results in a vector on the X-axis. */
BaserK 0:40b56bdec1d2 92 pitchAcc = atan2f((float)accelData[1], (float)accelData[2])*180/PI;
BaserK 0:40b56bdec1d2 93 rollAcc = atan2f((float)accelData[0], (float)accelData[2])*180/PI;
BaserK 0:40b56bdec1d2 94
BaserK 0:40b56bdec1d2 95 /* Apply Complementary Filter */
BaserK 0:40b56bdec1d2 96 *pitch = *pitch * 0.98 + pitchAcc * 0.02;
BaserK 0:40b56bdec1d2 97 *roll = *roll * 0.98 + rollAcc * 0.02;
BaserK 0:40b56bdec1d2 98 }