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@2:497faa1563ea, 2015-07-09 (annotated)
- Committer:
- BaserK
- Date:
- Thu Jul 09 12:28:26 2015 +0000
- Revision:
- 2:497faa1563ea
- Parent:
- 0:9203a021a0be
- Child:
- 3:88737ad5c803
Comment update;
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:9203a021a0be | 43 | |
BaserK | 0:9203a021a0be | 44 | void toggle_led1(); |
BaserK | 0:9203a021a0be | 45 | void toggle_led2(); |
BaserK | 0:9203a021a0be | 46 | |
BaserK | 0:9203a021a0be | 47 | int main() |
BaserK | 0:9203a021a0be | 48 | { |
BaserK | 0:9203a021a0be | 49 | ftdi.baud(9600); // baud rate: 9600 |
BaserK | 0:9203a021a0be | 50 | i2c.frequency(400000); // fast i2c: 400 kHz |
BaserK | 0:9203a021a0be | 51 | mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading |
BaserK | 0:9203a021a0be | 52 | wait(1); |
BaserK | 0:9203a021a0be | 53 | mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers |
BaserK | 0:9203a021a0be | 54 | ftdi.printf("Calibration is completed. \r\n"); |
BaserK | 0:9203a021a0be | 55 | wait(0.5); |
BaserK | 0:9203a021a0be | 56 | mpu6050.init(); // Initialize the sensor |
BaserK | 0:9203a021a0be | 57 | wait(1); |
BaserK | 0:9203a021a0be | 58 | ftdi.printf("MPU6050 is initialized for operation.. \r\n\r\n"); |
BaserK | 0:9203a021a0be | 59 | wait_ms(500); |
BaserK | 0:9203a021a0be | 60 | |
BaserK | 0:9203a021a0be | 61 | while(1) |
BaserK | 0:9203a021a0be | 62 | { |
BaserK | 0:9203a021a0be | 63 | /* Get actual acc value */ |
BaserK | 0:9203a021a0be | 64 | mpu6050.readAccelData(accelData); |
BaserK | 0:9203a021a0be | 65 | mpu6050.getAres(); |
BaserK | 0:9203a021a0be | 66 | ax = accelData[0]*aRes - accelBias[0]; |
BaserK | 0:9203a021a0be | 67 | ay = accelData[1]*aRes - accelBias[1]; |
BaserK | 0:9203a021a0be | 68 | az = accelData[2]*aRes - accelBias[2]; |
BaserK | 0:9203a021a0be | 69 | |
BaserK | 0:9203a021a0be | 70 | /* Get actual gyro value */ |
BaserK | 0:9203a021a0be | 71 | mpu6050.readGyroData(gyroData); |
BaserK | 0:9203a021a0be | 72 | mpu6050.getGres(); |
BaserK | 0:9203a021a0be | 73 | gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] |
BaserK | 0:9203a021a0be | 74 | gy = gyroData[1]*gRes; // - gyroBias[1]; |
BaserK | 0:9203a021a0be | 75 | gz = gyroData[2]*gRes; // - gyroBias[2]; |
BaserK | 0:9203a021a0be | 76 | |
BaserK | 0:9203a021a0be | 77 | ftdi.printf(" _____________________________________________________________ \r\n"); |
BaserK | 0:9203a021a0be | 78 | ftdi.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f \r\n",ax,ay,az); |
BaserK | 0:9203a021a0be | 79 | ftdi.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f \r\n",gx,gy,gz); |
BaserK | 0:9203a021a0be | 80 | ftdi.printf("|_____________________________________________________________ \r\n\r\n"); |
BaserK | 0:9203a021a0be | 81 | |
BaserK | 0:9203a021a0be | 82 | wait(2.5); |
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);} |