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@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?
User | Revision | Line number | New 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 | } |