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