Darley Gonzalez / Mbed OS MPU_6050_Hello_World

programa para hacer la lectura de un sensor MPU6050, el cual entrega el valor de gyro y del acelerómetro.

Dependencies:   MPU6050

main.cpp@0:1221112820f7, 2018-10-18 (annotated)

Committer:
213468891
Date:
Thu Oct 18 17:04:36 2018 +0000
Revision:
0:1221112820f7
Child:
1:4f9708c81c3a
Demonstrates how to read gyroscope and accelerometer values from the MPU-6050 module.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
213468891 0:1221112820f7 1 /*#################################################################################
213468891 0:1221112820f7 2
213468891 0:1221112820f7 3 Program Name : MPU6050 Hello World
213468891 0:1221112820f7 4 Author : Crispin Mukalay
213468891 0:1221112820f7 5 Date Modified : 17/10/2018
213468891 0:1221112820f7 6 Compiler : ARMmbed
213468891 0:1221112820f7 7 Tested On : NUCLEO-F446RE
213468891 0:1221112820f7 8
213468891 0:1221112820f7 9 Description : Demonstrates the use of the MPU6050 gryroscope/accelerometer/temperature
213468891 0:1221112820f7 10 sensor to read gyroscope 3-axis angular velocities(°/s) and accelerometer
213468891 0:1221112820f7 11 3-axis accelerations (°).
213468891 0:1221112820f7 12
213468891 0:1221112820f7 13 Requirements : * NUCLEO-F446RE Board
213468891 0:1221112820f7 14 * MPU6050 Module
213468891 0:1221112820f7 15
213468891 0:1221112820f7 16 Circuit : * The MPU6050 module is connected as follows:
213468891 0:1221112820f7 17 VCC - 3.3V
213468891 0:1221112820f7 18 GND - GND
213468891 0:1221112820f7 19 SCL - PB10 (I2C2_SCL pin)
213468891 0:1221112820f7 20 SDA - PB3 (I2C2_SDA pin)
213468891 0:1221112820f7 21
213468891 0:1221112820f7 22 ####################################################################################*/
213468891 0:1221112820f7 23
213468891 0:1221112820f7 24 #include "mbed.h"
213468891 0:1221112820f7 25 #include "MPU6050.h"
213468891 0:1221112820f7 26 #include <math.h>
213468891 0:1221112820f7 27 #define pi 3.141592654
213468891 0:1221112820f7 28
213468891 0:1221112820f7 29 MPU6050 AccGyro(PB_3, PB_10); // Create an MPU object called AccGyro
213468891 0:1221112820f7 30
213468891 0:1221112820f7 31 Serial pc(SERIAL_TX, SERIAL_RX); //To use the PC as a console (display output)
213468891 0:1221112820f7 32
213468891 0:1221112820f7 33 int16_t Ax, Ay, Az, Gx, Gy, Gz;
213468891 0:1221112820f7 34 float Ax_f, Ay_f, Az_f;
213468891 0:1221112820f7 35 double Gx_f, Gy_f, Gz_f;
213468891 0:1221112820f7 36 float Ax_f_sum, Ay_f_sum, Az_f_sum, Gx_f_sum, Gy_f_sum, Gz_f_sum;
213468891 0:1221112820f7 37 float roll, pitch, yaw;
213468891 0:1221112820f7 38
213468891 0:1221112820f7 39 int main() {
213468891 0:1221112820f7 40
213468891 0:1221112820f7 41 uint16_t AccelReadings[3] = {0, 0, 0};
213468891 0:1221112820f7 42 uint16_t GyroReadings[3] = {0, 0, 0};
213468891 0:1221112820f7 43 uint8_t DevId;
213468891 0:1221112820f7 44
213468891 0:1221112820f7 45 pc.printf("Starting MPU6050 test...\n");
213468891 0:1221112820f7 46 DevId = AccGyro.getWhoAmI();
213468891 0:1221112820f7 47
213468891 0:1221112820f7 48 if(DevId == 0x68){
213468891 0:1221112820f7 49 pc.printf("\n");
213468891 0:1221112820f7 50 pc.printf("MPU6050 detected...\n");
213468891 0:1221112820f7 51 pc.printf("Device ID is: 0x%02x\n", DevId);
213468891 0:1221112820f7 52 pc.printf("\n");
213468891 0:1221112820f7 53 }else{
213468891 0:1221112820f7 54 pc.printf("\n");
213468891 0:1221112820f7 55 pc.printf("MPU6050 not found...\n");
213468891 0:1221112820f7 56 while(1);
213468891 0:1221112820f7 57 }
213468891 0:1221112820f7 58
213468891 0:1221112820f7 59 // The device will come up in sleep mode upon power-up.
213468891 0:1221112820f7 60 AccGyro.setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC); // Disable sleep mode
213468891 0:1221112820f7 61 wait(.001);
213468891 0:1221112820f7 62
213468891 0:1221112820f7 63 // Full scale, +/-2000°/s, 16.4LSB°/s.
213468891 0:1221112820f7 64 AccGyro.setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Accelerometer elf-test trigger off.
213468891 0:1221112820f7 65 wait(.001);
213468891 0:1221112820f7 66
213468891 0:1221112820f7 67 // Full scale, +/-16g, 2048LSB/g.
213468891 0:1221112820f7 68 AccGyro.setAccelConfig(ACC_ST_OFF, AFS_16g); // Gyroscope self-test trigger off.
213468891 0:1221112820f7 69 wait(.001);
213468891 0:1221112820f7 70
213468891 0:1221112820f7 71 while (true) {
213468891 0:1221112820f7 72
213468891 0:1221112820f7 73 wait(0.2);
213468891 0:1221112820f7 74
213468891 0:1221112820f7 75 Ax_f_sum = 0;
213468891 0:1221112820f7 76 Ay_f_sum = 0;
213468891 0:1221112820f7 77 Az_f_sum = 0;
213468891 0:1221112820f7 78 Gx_f_sum = 0;
213468891 0:1221112820f7 79 Gy_f_sum = 0;
213468891 0:1221112820f7 80 Gz_f_sum = 0;
213468891 0:1221112820f7 81
213468891 0:1221112820f7 82 for(int i = 0; i < 10; i = i + 1) // Take ten analog input readings
213468891 0:1221112820f7 83 {
213468891 0:1221112820f7 84 AccGyro.readAccel(AccelReadings); // Extract accelerometer measurements
213468891 0:1221112820f7 85 AccGyro.readGyro(GyroReadings); // Extract gyroscope measurements
213468891 0:1221112820f7 86
213468891 0:1221112820f7 87 // 2s complement acclerometer and gyroscope values
213468891 0:1221112820f7 88 Ax = AccelReadings[0];
213468891 0:1221112820f7 89 Ay = AccelReadings[1];
213468891 0:1221112820f7 90 Az = AccelReadings[2];
213468891 0:1221112820f7 91 Gx = GyroReadings[0];
213468891 0:1221112820f7 92 Gy = GyroReadings[1];
213468891 0:1221112820f7 93 Gz = GyroReadings[2];
213468891 0:1221112820f7 94
213468891 0:1221112820f7 95 // Add every reading to the sum variables
213468891 0:1221112820f7 96 Ax_f_sum = Ax_f_sum + (float)Ax;
213468891 0:1221112820f7 97 Ay_f_sum = Ay_f_sum + (float)Ay;
213468891 0:1221112820f7 98 Az_f_sum = Az_f_sum + (float)Az;
213468891 0:1221112820f7 99 Gx_f_sum = Gx_f_sum + (float)Gx;
213468891 0:1221112820f7 100 Gy_f_sum = Gy_f_sum + (float)Gy;
213468891 0:1221112820f7 101 Gz_f_sum = Gz_f_sum + (float)Gz;
213468891 0:1221112820f7 102 }
213468891 0:1221112820f7 103
213468891 0:1221112820f7 104 // Divide by 10 to get the averaged value
213468891 0:1221112820f7 105 Ax_f = Ax_f_sum / 10;
213468891 0:1221112820f7 106 Ay_f = Ay_f_sum / 10;
213468891 0:1221112820f7 107 Az_f = Az_f_sum / 10;
213468891 0:1221112820f7 108 Gx_f = Gx_f_sum / 10;
213468891 0:1221112820f7 109 Gy_f = Gy_f_sum / 10;
213468891 0:1221112820f7 110 Gz_f = Gz_f_sum / 10;
213468891 0:1221112820f7 111
213468891 0:1221112820f7 112 // 1. Calculate actual roll, pitch and yaw angles in degrees
213468891 0:1221112820f7 113 // 2. Calibrate readings by adding or substracting the off-set
213468891 0:1221112820f7 114 roll = (180/pi)*(atan(Ax_f/(sqrt((Ay_f*Ay_f)+(Az_f*Az_f))))) - 4.36;
213468891 0:1221112820f7 115 pitch = (180/pi)*(atan(Ay_f/(sqrt((Ax_f*Ax_f)+(Az_f*Az_f))))) - 0.063;
213468891 0:1221112820f7 116 yaw = (180/pi)*(atan((sqrt((Ax_f*Ax_f)+(Ay_f*Ay_f)))/Az_f)) - 3.93;
213468891 0:1221112820f7 117
213468891 0:1221112820f7 118 // Convert gyroscope readings into degrees/s
213468891 0:1221112820f7 119 Gx_f = Gx_f / 131.0;
213468891 0:1221112820f7 120 Gy_f = Gy_f / 131.0;
213468891 0:1221112820f7 121 Gz_f = Gz_f / 131.0;
213468891 0:1221112820f7 122
213468891 0:1221112820f7 123 pc.printf("Gyro(deg/s) X: %.3f Y: %.3f Z: %.3f || Accel(deg) Roll: %.3f, Pitch: %.3f, Yaw: %.3f \n", Gx_f, Gy_f, Gz_f, roll, pitch, yaw);
213468891 0:1221112820f7 124
213468891 0:1221112820f7 125 }
213468891 0:1221112820f7 126 }
213468891 0:1221112820f7 127