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@6:12ff524abb6a, 2015-08-05 (annotated)

Committer:
BaserK
Date:
Wed Aug 05 12:23:06 2015 +0000
Revision:
6:12ff524abb6a
Parent:
5:a6a235d5442d
Child:
7:3e1e78b34e1a
Compatible with NUCLEO F411RE

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 6:12ff524abb6a 44 *--------------------------------------------------------------
BaserK 6:12ff524abb6a 45 * |NUCLEO F411RE| |Peripherals|
BaserK 6:12ff524abb6a 46 * D14 -----------> SDA of MPU6050
BaserK 6:12ff524abb6a 47 * D15 -----------> SCL of MPU6050
BaserK 6:12ff524abb6a 48 * GND -----------> GND of MPU6050
BaserK 6:12ff524abb6a 49 * VOUT (3.3 V) --> VCC of MPU6050
BaserK 6:12ff524abb6a 50 *---------------------------------------------------------------
BaserK 6:12ff524abb6a 51
BaserK 6:12ff524abb6a 52
BaserK 0:9203a021a0be 53 * Note: For any mistakes or comments, please contact me.
BaserK 0:9203a021a0be 54 */
BaserK 0:9203a021a0be 55
BaserK 0:9203a021a0be 56 #include "mbed.h"
BaserK 0:9203a021a0be 57 #include "MPU6050.h"
BaserK 0:9203a021a0be 58 #include "ledControl.h"
BaserK 0:9203a021a0be 59
BaserK 0:9203a021a0be 60 /* */
BaserK 0:9203a021a0be 61
BaserK 0:9203a021a0be 62 /* Defined in the MPU6050.cpp file */
BaserK 0:9203a021a0be 63 // I2C i2c(p9,p10); // setup i2c (SDA,SCL)
BaserK 0:9203a021a0be 64
BaserK 4:33fef1998fc8 65 Serial pc(USBTX,USBRX); // default baud rate: 9600
BaserK 4:33fef1998fc8 66 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
BaserK 0:9203a021a0be 67 Ticker toggler1;
BaserK 3:88737ad5c803 68 Ticker filter;
BaserK 0:9203a021a0be 69
BaserK 0:9203a021a0be 70 void toggle_led1();
BaserK 0:9203a021a0be 71 void toggle_led2();
BaserK 3:88737ad5c803 72 void compFilter();
BaserK 3:88737ad5c803 73
BaserK 3:88737ad5c803 74 float pitchAngle = 0;
BaserK 3:88737ad5c803 75 float rollAngle = 0;
BaserK 0:9203a021a0be 76
BaserK 0:9203a021a0be 77 int main()
BaserK 0:9203a021a0be 78 {
BaserK 4:33fef1998fc8 79 pc.baud(9600); // baud rate: 9600
BaserK 0:9203a021a0be 80 i2c.frequency(400000); // fast i2c: 400 kHz
BaserK 0:9203a021a0be 81 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:9203a021a0be 82 wait(1);
BaserK 0:9203a021a0be 83 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 4:33fef1998fc8 84 pc.printf("Calibration is completed. \r\n");
BaserK 0:9203a021a0be 85 wait(0.5);
BaserK 0:9203a021a0be 86 mpu6050.init(); // Initialize the sensor
BaserK 0:9203a021a0be 87 wait(1);
BaserK 4:33fef1998fc8 88 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
BaserK 0:9203a021a0be 89 wait_ms(500);
BaserK 0:9203a021a0be 90
BaserK 0:9203a021a0be 91 while(1)
BaserK 0:9203a021a0be 92 {
BaserK 4:33fef1998fc8 93
BaserK 4:33fef1998fc8 94 /* Uncomment below if you want to see accel and gyro data */
BaserK 4:33fef1998fc8 95
BaserK 4:33fef1998fc8 96 // pc.printf(" _____________________________________________________________ \r\n");
BaserK 4:33fef1998fc8 97 // pc.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az);
BaserK 4:33fef1998fc8 98 // pc.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz);
BaserK 4:33fef1998fc8 99 // pc.printf("|_____________________________________________________________ \r\n\r\n");
BaserK 3:88737ad5c803 100 //
BaserK 3:88737ad5c803 101 // wait(2.5);
BaserK 3:88737ad5c803 102
BaserK 3:88737ad5c803 103 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:9203a021a0be 104
BaserK 4:33fef1998fc8 105 pc.printf(" _______________\r\n");
BaserK 4:33fef1998fc8 106 pc.printf("| Pitch: %.3f \r\n",pitchAngle);
BaserK 4:33fef1998fc8 107 pc.printf("| Roll: %.3f \r\n",rollAngle);
BaserK 4:33fef1998fc8 108 pc.printf("|_______________\r\n\r\n");
BaserK 0:9203a021a0be 109
BaserK 3:88737ad5c803 110 wait(1);
BaserK 4:33fef1998fc8 111
BaserK 0:9203a021a0be 112 }
BaserK 0:9203a021a0be 113 }
BaserK 0:9203a021a0be 114
BaserK 0:9203a021a0be 115 void toggle_led1() {ledToggle(1);}
BaserK 0:9203a021a0be 116 void toggle_led2() {ledToggle(2);}
BaserK 3:88737ad5c803 117
BaserK 3:88737ad5c803 118 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 4:33fef1998fc8 119 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}