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@5:a6a235d5442d, 2015-07-21 (annotated)

Committer:
BaserK
Date:
Tue Jul 21 08:19:59 2015 +0000
Revision:
5:a6a235d5442d
Parent:
4:33fef1998fc8
Child:
6:12ff524abb6a
MIT license is added

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 5:a6a235d5442d 5 * @license: MIT license
BaserK 5:a6a235d5442d 6 *
BaserK 5:a6a235d5442d 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 5:a6a235d5442d 8 *
BaserK 5:a6a235d5442d 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 5:a6a235d5442d 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 5:a6a235d5442d 11 * in the Software without restriction, including without limitation the rights
BaserK 5:a6a235d5442d 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 5:a6a235d5442d 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 5:a6a235d5442d 14 * furnished to do so, subject to the following conditions:
BaserK 5:a6a235d5442d 15 *
BaserK 5:a6a235d5442d 16 * The above copyright notice and this permission notice shall be included in
BaserK 5:a6a235d5442d 17 * all copies or substantial portions of the Software.
BaserK 5:a6a235d5442d 18 *
BaserK 5:a6a235d5442d 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 5:a6a235d5442d 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 5:a6a235d5442d 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 5:a6a235d5442d 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 5:a6a235d5442d 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 5:a6a235d5442d 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 5:a6a235d5442d 25 * THE SOFTWARE.
BaserK 0:9203a021a0be 26 *
BaserK 0:9203a021a0be 27 * @description of the program:
BaserK 0:9203a021a0be 28 *
BaserK 2:497faa1563ea 29 * First of all most of the credit goes to Kris Winer for his useful MPU6050 code.
BaserK 0:9203a021a0be 30 * I rewrite the code in my way using class prototypes and my comments. This program
BaserK 0:9203a021a0be 31 * can be a starter point for more advanced projects including quadcopters, balancing
BaserK 0:9203a021a0be 32 * robots etc. Program takes accelerometer and gyroscope data from the MPU6050 registers
BaserK 4:33fef1998fc8 33 * and calibrates them for better results. Then uses this data is to obtain pitch and roll
BaserK 4:33fef1998fc8 34 * angles and writes these angles to the terminal which mbed is connected to.
BaserK 0:9203a021a0be 35 *
BaserK 0:9203a021a0be 36 * @connections:
BaserK 0:9203a021a0be 37 *--------------------------------------------------------------
BaserK 0:9203a021a0be 38 * |LPC1768| |Peripherals|
BaserK 0:9203a021a0be 39 * Pin 9 ---------> SDA of MPU6050
BaserK 0:9203a021a0be 40 * Pin 10 --------> SCL of MPU6050
BaserK 4:33fef1998fc8 41 * GND -----------> GND of MPU6050
BaserK 0:9203a021a0be 42 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 0:9203a021a0be 43 *---------------------------------------------------------------
BaserK 0:9203a021a0be 44 * Note: For any mistakes or comments, please contact me.
BaserK 0:9203a021a0be 45 */
BaserK 0:9203a021a0be 46
BaserK 0:9203a021a0be 47 #include "mbed.h"
BaserK 0:9203a021a0be 48 #include "MPU6050.h"
BaserK 0:9203a021a0be 49 #include "ledControl.h"
BaserK 0:9203a021a0be 50
BaserK 0:9203a021a0be 51 /* */
BaserK 0:9203a021a0be 52
BaserK 0:9203a021a0be 53 /* Defined in the MPU6050.cpp file */
BaserK 0:9203a021a0be 54 // I2C i2c(p9,p10); // setup i2c (SDA,SCL)
BaserK 0:9203a021a0be 55
BaserK 4:33fef1998fc8 56 Serial pc(USBTX,USBRX); // default baud rate: 9600
BaserK 4:33fef1998fc8 57 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
BaserK 0:9203a021a0be 58 Ticker toggler1;
BaserK 3:88737ad5c803 59 Ticker filter;
BaserK 0:9203a021a0be 60
BaserK 0:9203a021a0be 61 void toggle_led1();
BaserK 0:9203a021a0be 62 void toggle_led2();
BaserK 3:88737ad5c803 63 void compFilter();
BaserK 3:88737ad5c803 64
BaserK 3:88737ad5c803 65 float pitchAngle = 0;
BaserK 3:88737ad5c803 66 float rollAngle = 0;
BaserK 0:9203a021a0be 67
BaserK 0:9203a021a0be 68 int main()
BaserK 0:9203a021a0be 69 {
BaserK 4:33fef1998fc8 70 pc.baud(9600); // baud rate: 9600
BaserK 0:9203a021a0be 71 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 0:9203a021a0be 72 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:9203a021a0be 73 wait(1);
BaserK 0:9203a021a0be 74 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 4:33fef1998fc8 75 pc.printf("Calibration is completed. \r\n");
BaserK 0:9203a021a0be 76 wait(0.5);
BaserK 0:9203a021a0be 77 mpu6050.init(); // Initialize the sensor
BaserK 0:9203a021a0be 78 wait(1);
BaserK 4:33fef1998fc8 79 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:9203a021a0be 80 wait_ms(500);
BaserK 0:9203a021a0be 81
BaserK 0:9203a021a0be 82 while(1)
BaserK 0:9203a021a0be 83 {
BaserK 4:33fef1998fc8 84
BaserK 4:33fef1998fc8 85 /* Uncomment below if you want to see accel and gyro data */
BaserK 4:33fef1998fc8 86
BaserK 4:33fef1998fc8 87 // pc.printf(" _____________________________________________________________ \r\n");
BaserK 4:33fef1998fc8 88 // pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
BaserK 4:33fef1998fc8 89 // pc.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
BaserK 4:33fef1998fc8 90 // pc.printf("|_____________________________________________________________ \r\n\r\n");
BaserK 3:88737ad5c803 91 //
BaserK 3:88737ad5c803 92 // wait(2.5);
BaserK 3:88737ad5c803 93
BaserK 3:88737ad5c803 94 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:9203a021a0be 95
BaserK 4:33fef1998fc8 96 pc.printf(" _______________\r\n");
BaserK 4:33fef1998fc8 97 pc.printf("| Pitch: %.3f \r\n",pitchAngle);
BaserK 4:33fef1998fc8 98 pc.printf("| Roll: %.3f \r\n",rollAngle);
BaserK 4:33fef1998fc8 99 pc.printf("|_______________\r\n\r\n");
BaserK 0:9203a021a0be 100
BaserK 3:88737ad5c803 101 wait(1);
BaserK 4:33fef1998fc8 102
BaserK 0:9203a021a0be 103 }
BaserK 0:9203a021a0be 104 }
BaserK 0:9203a021a0be 105
BaserK 0:9203a021a0be 106 void toggle_led1() {ledToggle(1);}
BaserK 0:9203a021a0be 107 void toggle_led2() {ledToggle(2);}
BaserK 3:88737ad5c803 108
BaserK 3:88737ad5c803 109 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 4:33fef1998fc8 110 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}