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@4:33fef1998fc8, 2015-07-16 (annotated)

Committer:
BaserK
Date:
Thu Jul 16 13:58:38 2015 +0000
Revision:
4:33fef1998fc8
Parent:
3:88737ad5c803
Child:
5:a6a235d5442d
The main program now uses complementary filter as a function to calculate pitch and roll angles

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 4:33fef1998fc8 1 /* Getting Pitch and Roll Angles from MPU6050
BaserK 0:9203a021a0be 2 *
BaserK 0:9203a021a0be 3 * @author: Baser Kandehir
BaserK 4:33fef1998fc8 4 * @date: July 16, 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 4:33fef1998fc8 13 * and calibrates them for better results. Then uses this data is to obtain pitch and roll
BaserK 4:33fef1998fc8 14 * angles and writes these angles to the terminal which mbed is connected to.
BaserK 0:9203a021a0be 15 *
BaserK 0:9203a021a0be 16 * @connections:
BaserK 0:9203a021a0be 17 *--------------------------------------------------------------
BaserK 0:9203a021a0be 18 * |LPC1768| |Peripherals|
BaserK 0:9203a021a0be 19 * Pin 9 ---------> SDA of MPU6050
BaserK 0:9203a021a0be 20 * Pin 10 --------> SCL of MPU6050
BaserK 4:33fef1998fc8 21 * GND -----------> GND of MPU6050
BaserK 0:9203a021a0be 22 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 0:9203a021a0be 23 *---------------------------------------------------------------
BaserK 0:9203a021a0be 24 * Note: For any mistakes or comments, please contact me.
BaserK 0:9203a021a0be 25 */
BaserK 0:9203a021a0be 26
BaserK 0:9203a021a0be 27 #include "mbed.h"
BaserK 0:9203a021a0be 28 #include "MPU6050.h"
BaserK 0:9203a021a0be 29 #include "ledControl.h"
BaserK 0:9203a021a0be 30
BaserK 0:9203a021a0be 31 /* */
BaserK 0:9203a021a0be 32
BaserK 0:9203a021a0be 33 /* Defined in the MPU6050.cpp file */
BaserK 0:9203a021a0be 34 // I2C i2c(p9,p10); // setup i2c (SDA,SCL)
BaserK 0:9203a021a0be 35
BaserK 4:33fef1998fc8 36 Serial pc(USBTX,USBRX); // default baud rate: 9600
BaserK 4:33fef1998fc8 37 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
BaserK 0:9203a021a0be 38 Ticker toggler1;
BaserK 3:88737ad5c803 39 Ticker filter;
BaserK 0:9203a021a0be 40
BaserK 0:9203a021a0be 41 void toggle_led1();
BaserK 0:9203a021a0be 42 void toggle_led2();
BaserK 3:88737ad5c803 43 void compFilter();
BaserK 3:88737ad5c803 44
BaserK 3:88737ad5c803 45 float pitchAngle = 0;
BaserK 3:88737ad5c803 46 float rollAngle = 0;
BaserK 0:9203a021a0be 47
BaserK 0:9203a021a0be 48 int main()
BaserK 0:9203a021a0be 49 {
BaserK 4:33fef1998fc8 50 pc.baud(9600); // baud rate: 9600
BaserK 0:9203a021a0be 51 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 0:9203a021a0be 52 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:9203a021a0be 53 wait(1);
BaserK 0:9203a021a0be 54 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 4:33fef1998fc8 55 pc.printf("Calibration is completed. \r\n");
BaserK 0:9203a021a0be 56 wait(0.5);
BaserK 0:9203a021a0be 57 mpu6050.init(); // Initialize the sensor
BaserK 0:9203a021a0be 58 wait(1);
BaserK 4:33fef1998fc8 59 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:9203a021a0be 60 wait_ms(500);
BaserK 0:9203a021a0be 61
BaserK 0:9203a021a0be 62 while(1)
BaserK 0:9203a021a0be 63 {
BaserK 4:33fef1998fc8 64
BaserK 4:33fef1998fc8 65 /* Uncomment below if you want to see accel and gyro data */
BaserK 4:33fef1998fc8 66
BaserK 4:33fef1998fc8 67 // pc.printf(" _____________________________________________________________ \r\n");
BaserK 4:33fef1998fc8 68 // pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
BaserK 4:33fef1998fc8 69 // pc.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
BaserK 4:33fef1998fc8 70 // pc.printf("|_____________________________________________________________ \r\n\r\n");
BaserK 3:88737ad5c803 71 //
BaserK 3:88737ad5c803 72 // wait(2.5);
BaserK 3:88737ad5c803 73
BaserK 3:88737ad5c803 74 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:9203a021a0be 75
BaserK 4:33fef1998fc8 76 pc.printf(" _______________\r\n");
BaserK 4:33fef1998fc8 77 pc.printf("| Pitch: %.3f \r\n",pitchAngle);
BaserK 4:33fef1998fc8 78 pc.printf("| Roll: %.3f \r\n",rollAngle);
BaserK 4:33fef1998fc8 79 pc.printf("|_______________\r\n\r\n");
BaserK 0:9203a021a0be 80
BaserK 3:88737ad5c803 81 wait(1);
BaserK 4:33fef1998fc8 82
BaserK 0:9203a021a0be 83 }
BaserK 0:9203a021a0be 84 }
BaserK 0:9203a021a0be 85
BaserK 0:9203a021a0be 86 void toggle_led1() {ledToggle(1);}
BaserK 0:9203a021a0be 87 void toggle_led2() {ledToggle(2);}
BaserK 3:88737ad5c803 88
BaserK 3:88737ad5c803 89 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 4:33fef1998fc8 90 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}