Baser Kandehir / Mbed 2 deprecated i2c_MPU6050 Featured

Program takes accelerometer and gyroscope data from the MPU6050 registers and calibrates them for better results. Then uses this data is to obtain pitch and roll angles and writes these angles to the terminal which mbed is connected to.

Dependencies:   MPU6050 ledControl2 mbed

main.cpp@3:88737ad5c803, 2015-07-13 (annotated)

Committer:
BaserK
Date:
Mon Jul 13 13:19:08 2015 +0000
Revision:
3:88737ad5c803
Parent:
2:497faa1563ea
Child:
4:33fef1998fc8
Complementary filter and pitch, roll angles are added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:9203a021a0be 1 /* MPU6050 example with I2C interface on LPC1768
BaserK 0:9203a021a0be 2 *
BaserK 0:9203a021a0be 3 * @author: Baser Kandehir
BaserK 0:9203a021a0be 4 * @date: July 9, 2015
BaserK 0:9203a021a0be 5 * @license: Use this code however you'd like
BaserK 0:9203a021a0be 6 *
BaserK 0:9203a021a0be 7 * @description of the program:
BaserK 0:9203a021a0be 8 *
BaserK 2:497faa1563ea 9 * First of all most of the credit goes to Kris Winer for his useful MPU6050 code.
BaserK 0:9203a021a0be 10 * I rewrite the code in my way using class prototypes and my comments. This program
BaserK 0:9203a021a0be 11 * can be a starter point for more advanced projects including quadcopters, balancing
BaserK 0:9203a021a0be 12 * robots etc. Program takes accelerometer and gyroscope data from the MPU6050 registers
BaserK 0:9203a021a0be 13 * and calibrates them for better results. And writes accel and gyro x,y,z data to the
BaserK 0:9203a021a0be 14 * terminal. I will use this code later on for sensor fusion with a compass to get pitch,
BaserK 0:9203a021a0be 15 * roll and yaw angles. After that I am planning to use some filtering algorithms like
BaserK 0:9203a021a0be 16 * Kalman Filter and Complementary filter. I will keep updating the code as I write them.
BaserK 0:9203a021a0be 17 *
BaserK 0:9203a021a0be 18 * @connections:
BaserK 0:9203a021a0be 19 *--------------------------------------------------------------
BaserK 0:9203a021a0be 20 * |LPC1768| |Peripherals|
BaserK 0:9203a021a0be 21 * Pin 9 ---------> SDA of MPU6050
BaserK 0:9203a021a0be 22 * Pin 10 --------> SCL of MPU6050
BaserK 0:9203a021a0be 23 * Pin 13 --------> (TX) RX pin of the FTDI or Bluetooth etc.
BaserK 0:9203a021a0be 24 * Pin 14 --------> (RX) TX pin of the FTDI or Bluetooth etc.
BaserK 0:9203a021a0be 25 * GND -----------> GND of any peripheral
BaserK 0:9203a021a0be 26 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 0:9203a021a0be 27 *---------------------------------------------------------------
BaserK 0:9203a021a0be 28 * Note: For any mistakes or comments, please contact me.
BaserK 0:9203a021a0be 29 */
BaserK 0:9203a021a0be 30
BaserK 0:9203a021a0be 31 #include "mbed.h"
BaserK 0:9203a021a0be 32 #include "MPU6050.h"
BaserK 0:9203a021a0be 33 #include "ledControl.h"
BaserK 0:9203a021a0be 34
BaserK 0:9203a021a0be 35 /* */
BaserK 0:9203a021a0be 36
BaserK 0:9203a021a0be 37 /* Defined in the MPU6050.cpp file */
BaserK 0:9203a021a0be 38 // I2C i2c(p9,p10); // setup i2c (SDA,SCL)
BaserK 0:9203a021a0be 39
BaserK 0:9203a021a0be 40 Serial ftdi(p13,p14); // default baud rate: 9600
BaserK 0:9203a021a0be 41 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
BaserK 0:9203a021a0be 42 Ticker toggler1;
BaserK 3:88737ad5c803 43 Ticker filter;
BaserK 0:9203a021a0be 44
BaserK 0:9203a021a0be 45 void toggle_led1();
BaserK 0:9203a021a0be 46 void toggle_led2();
BaserK 3:88737ad5c803 47 void complementaryFilter(float* pitch, float* roll);
BaserK 3:88737ad5c803 48 void compFilter();
BaserK 3:88737ad5c803 49
BaserK 3:88737ad5c803 50 float pitchAngle = 0;
BaserK 3:88737ad5c803 51 float rollAngle = 0;
BaserK 0:9203a021a0be 52
BaserK 0:9203a021a0be 53 int main()
BaserK 0:9203a021a0be 54 {
BaserK 0:9203a021a0be 55 ftdi.baud(9600); // baud rate: 9600
BaserK 0:9203a021a0be 56 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 0:9203a021a0be 57 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:9203a021a0be 58 wait(1);
BaserK 0:9203a021a0be 59 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 0:9203a021a0be 60 ftdi.printf("Calibration is completed. \r\n");
BaserK 0:9203a021a0be 61 wait(0.5);
BaserK 0:9203a021a0be 62 mpu6050.init(); // Initialize the sensor
BaserK 0:9203a021a0be 63 wait(1);
BaserK 0:9203a021a0be 64 ftdi.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:9203a021a0be 65 wait_ms(500);
BaserK 0:9203a021a0be 66
BaserK 0:9203a021a0be 67 while(1)
BaserK 0:9203a021a0be 68 {
BaserK 3:88737ad5c803 69 // /* Get actual acc value */
BaserK 3:88737ad5c803 70 // mpu6050.readAccelData(accelData);
BaserK 3:88737ad5c803 71 // mpu6050.getAres();
BaserK 3:88737ad5c803 72 // ax = accelData[0]*aRes - accelBias[0];
BaserK 3:88737ad5c803 73 // ay = accelData[1]*aRes - accelBias[1];
BaserK 3:88737ad5c803 74 // az = accelData[2]*aRes - accelBias[2];
BaserK 3:88737ad5c803 75 //
BaserK 3:88737ad5c803 76 // /* Get actual gyro value */
BaserK 3:88737ad5c803 77 // mpu6050.readGyroData(gyroData);
BaserK 3:88737ad5c803 78 // mpu6050.getGres();
BaserK 3:88737ad5c803 79 // gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i]
BaserK 3:88737ad5c803 80 // gy = gyroData[1]*gRes; // - gyroBias[1];
BaserK 3:88737ad5c803 81 // gz = gyroData[2]*gRes; // - gyroBias[2];
BaserK 3:88737ad5c803 82 //
BaserK 3:88737ad5c803 83 // ftdi.printf(" _____________________________________________________________ \r\n");
BaserK 3:88737ad5c803 84 // ftdi.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
BaserK 3:88737ad5c803 85 // ftdi.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
BaserK 3:88737ad5c803 86 // ftdi.printf("|_____________________________________________________________ \r\n\r\n");
BaserK 3:88737ad5c803 87 //
BaserK 3:88737ad5c803 88 // wait(2.5);
BaserK 3:88737ad5c803 89
BaserK 3:88737ad5c803 90 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:9203a021a0be 91
BaserK 3:88737ad5c803 92 ftdi.printf(" _______________\r\n");
BaserK 3:88737ad5c803 93 ftdi.printf("| Pitch: %.3f \r\n",pitchAngle);
BaserK 3:88737ad5c803 94 ftdi.printf("| Roll: %.3f \r\n",rollAngle);
BaserK 3:88737ad5c803 95 ftdi.printf("|_______________\r\n\r\n");
BaserK 0:9203a021a0be 96
BaserK 3:88737ad5c803 97 wait(1);
BaserK 0:9203a021a0be 98 }
BaserK 0:9203a021a0be 99 }
BaserK 0:9203a021a0be 100
BaserK 0:9203a021a0be 101 void toggle_led1() {ledToggle(1);}
BaserK 0:9203a021a0be 102 void toggle_led2() {ledToggle(2);}
BaserK 3:88737ad5c803 103
BaserK 3:88737ad5c803 104 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 3:88737ad5c803 105 void compFilter() {complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 3:88737ad5c803 106
BaserK 3:88737ad5c803 107 void complementaryFilter(float* pitch, float* roll)
BaserK 3:88737ad5c803 108 {
BaserK 3:88737ad5c803 109 /* Get actual acc value */
BaserK 3:88737ad5c803 110 mpu6050.readAccelData(accelData);
BaserK 3:88737ad5c803 111 mpu6050.getAres();
BaserK 3:88737ad5c803 112 ax = accelData[0]*aRes - accelBias[0];
BaserK 3:88737ad5c803 113 ay = accelData[1]*aRes - accelBias[1];
BaserK 3:88737ad5c803 114 az = accelData[2]*aRes - accelBias[2];
BaserK 3:88737ad5c803 115
BaserK 3:88737ad5c803 116 /* Get actual gyro value */
BaserK 3:88737ad5c803 117 mpu6050.readGyroData(gyroData);
BaserK 3:88737ad5c803 118 mpu6050.getGres();
BaserK 3:88737ad5c803 119 gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i]
BaserK 3:88737ad5c803 120 gy = gyroData[1]*gRes; // - gyroBias[1];
BaserK 3:88737ad5c803 121 gz = gyroData[2]*gRes; // - gyroBias[2];
BaserK 3:88737ad5c803 122
BaserK 3:88737ad5c803 123 float pitchAcc, rollAcc;
BaserK 3:88737ad5c803 124
BaserK 3:88737ad5c803 125 /* Integrate the gyro data(deg/s) over time to get angle */
BaserK 3:88737ad5c803 126 *pitch += gx * dt; // Angle around the X-axis
BaserK 3:88737ad5c803 127 *roll -= gy * dt; // Angle around the Y-axis
BaserK 3:88737ad5c803 128
BaserK 3:88737ad5c803 129 /* Turning around the X-axis results in a vector on the Y-axis
BaserK 3:88737ad5c803 130 whereas turning around the Y-axis results in a vector on the X-axis. */
BaserK 3:88737ad5c803 131 pitchAcc = atan2f((float)accelData[1], (float)accelData[2])*180/PI;
BaserK 3:88737ad5c803 132 rollAcc = atan2f((float)accelData[0], (float)accelData[2])*180/PI;
BaserK 3:88737ad5c803 133
BaserK 3:88737ad5c803 134 /* Apply Complementary Filter */
BaserK 3:88737ad5c803 135 *pitch = *pitch * 0.98 + pitchAcc * 0.02;
BaserK 3:88737ad5c803 136 *roll = *roll * 0.98 + rollAcc * 0.02;
BaserK 3:88737ad5c803 137 }