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@1:4f9708c81c3a, 2021-03-01 (annotated)

Committer:
darley_gonzalez
Date:
Mon Mar 01 22:33:58 2021 +0000
Revision:
1:4f9708c81c3a
Parent:
0:1221112820f7 
MPU6050

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
darley_gonzalez 1:4f9708c81c3a 29 //MPU6050 AccGyro(PTC2, PTC1); // Create an MPU object called AccGyro
darley_gonzalez 1:4f9708c81c3a 30 MPU6050 AccGyro(I2C_SDA, I2C_SCL); // Create an MPU object called AccGyro
213468891 0:1221112820f7 31
darley_gonzalez 1:4f9708c81c3a 32 Serial pc(USBTX, USBRX); //To use the PC as a console (display output)
213468891 0:1221112820f7 33
213468891 0:1221112820f7 34 int16_t Ax, Ay, Az, Gx, Gy, Gz;
213468891 0:1221112820f7 35 float Ax_f, Ay_f, Az_f;
213468891 0:1221112820f7 36 double Gx_f, Gy_f, Gz_f;
213468891 0:1221112820f7 37 float Ax_f_sum, Ay_f_sum, Az_f_sum, Gx_f_sum, Gy_f_sum, Gz_f_sum;
213468891 0:1221112820f7 38 float roll, pitch, yaw;
213468891 0:1221112820f7 39
213468891 0:1221112820f7 40 int main() {
213468891 0:1221112820f7 41
213468891 0:1221112820f7 42 uint16_t AccelReadings[3] = {0, 0, 0};
213468891 0:1221112820f7 43 uint16_t GyroReadings[3] = {0, 0, 0};
213468891 0:1221112820f7 44 uint8_t DevId;
213468891 0:1221112820f7 45
213468891 0:1221112820f7 46 pc.printf("Starting MPU6050 test...\n");
213468891 0:1221112820f7 47 DevId = AccGyro.getWhoAmI();
213468891 0:1221112820f7 48
213468891 0:1221112820f7 49 if(DevId == 0x68){
213468891 0:1221112820f7 50 pc.printf("\n");
213468891 0:1221112820f7 51 pc.printf("MPU6050 detected...\n");
213468891 0:1221112820f7 52 pc.printf("Device ID is: 0x%02x\n", DevId);
213468891 0:1221112820f7 53 pc.printf("\n");
213468891 0:1221112820f7 54 }else{
213468891 0:1221112820f7 55 pc.printf("\n");
213468891 0:1221112820f7 56 pc.printf("MPU6050 not found...\n");
213468891 0:1221112820f7 57 while(1);
213468891 0:1221112820f7 58 }
213468891 0:1221112820f7 59
213468891 0:1221112820f7 60 // The device will come up in sleep mode upon power-up.
213468891 0:1221112820f7 61 AccGyro.setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC); // Disable sleep mode
213468891 0:1221112820f7 62 wait(.001);
213468891 0:1221112820f7 63
213468891 0:1221112820f7 64 // Full scale, +/-2000°/s, 16.4LSB°/s.
213468891 0:1221112820f7 65 AccGyro.setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Accelerometer elf-test trigger off.
213468891 0:1221112820f7 66 wait(.001);
213468891 0:1221112820f7 67
213468891 0:1221112820f7 68 // Full scale, +/-16g, 2048LSB/g.
213468891 0:1221112820f7 69 AccGyro.setAccelConfig(ACC_ST_OFF, AFS_16g); // Gyroscope self-test trigger off.
213468891 0:1221112820f7 70 wait(.001);
213468891 0:1221112820f7 71
213468891 0:1221112820f7 72 while (true) {
213468891 0:1221112820f7 73
213468891 0:1221112820f7 74 wait(0.2);
213468891 0:1221112820f7 75
213468891 0:1221112820f7 76 Ax_f_sum = 0;
213468891 0:1221112820f7 77 Ay_f_sum = 0;
213468891 0:1221112820f7 78 Az_f_sum = 0;
213468891 0:1221112820f7 79 Gx_f_sum = 0;
213468891 0:1221112820f7 80 Gy_f_sum = 0;
213468891 0:1221112820f7 81 Gz_f_sum = 0;
213468891 0:1221112820f7 82
213468891 0:1221112820f7 83 for(int i = 0; i < 10; i = i + 1) // Take ten analog input readings
213468891 0:1221112820f7 84 {
213468891 0:1221112820f7 85 AccGyro.readAccel(AccelReadings); // Extract accelerometer measurements
213468891 0:1221112820f7 86 AccGyro.readGyro(GyroReadings); // Extract gyroscope measurements
213468891 0:1221112820f7 87
213468891 0:1221112820f7 88 // 2s complement acclerometer and gyroscope values
213468891 0:1221112820f7 89 Ax = AccelReadings[0];
213468891 0:1221112820f7 90 Ay = AccelReadings[1];
213468891 0:1221112820f7 91 Az = AccelReadings[2];
213468891 0:1221112820f7 92 Gx = GyroReadings[0];
213468891 0:1221112820f7 93 Gy = GyroReadings[1];
213468891 0:1221112820f7 94 Gz = GyroReadings[2];
213468891 0:1221112820f7 95
213468891 0:1221112820f7 96 // Add every reading to the sum variables
213468891 0:1221112820f7 97 Ax_f_sum = Ax_f_sum + (float)Ax;
213468891 0:1221112820f7 98 Ay_f_sum = Ay_f_sum + (float)Ay;
213468891 0:1221112820f7 99 Az_f_sum = Az_f_sum + (float)Az;
213468891 0:1221112820f7 100 Gx_f_sum = Gx_f_sum + (float)Gx;
213468891 0:1221112820f7 101 Gy_f_sum = Gy_f_sum + (float)Gy;
213468891 0:1221112820f7 102 Gz_f_sum = Gz_f_sum + (float)Gz;
213468891 0:1221112820f7 103 }
213468891 0:1221112820f7 104
213468891 0:1221112820f7 105 // Divide by 10 to get the averaged value
213468891 0:1221112820f7 106 Ax_f = Ax_f_sum / 10;
213468891 0:1221112820f7 107 Ay_f = Ay_f_sum / 10;
213468891 0:1221112820f7 108 Az_f = Az_f_sum / 10;
213468891 0:1221112820f7 109 Gx_f = Gx_f_sum / 10;
213468891 0:1221112820f7 110 Gy_f = Gy_f_sum / 10;
213468891 0:1221112820f7 111 Gz_f = Gz_f_sum / 10;
213468891 0:1221112820f7 112
213468891 0:1221112820f7 113 // 1. Calculate actual roll, pitch and yaw angles in degrees
213468891 0:1221112820f7 114 // 2. Calibrate readings by adding or substracting the off-set
213468891 0:1221112820f7 115 roll = (180/pi)*(atan(Ax_f/(sqrt((Ay_f*Ay_f)+(Az_f*Az_f))))) - 4.36;
213468891 0:1221112820f7 116 pitch = (180/pi)*(atan(Ay_f/(sqrt((Ax_f*Ax_f)+(Az_f*Az_f))))) - 0.063;
213468891 0:1221112820f7 117 yaw = (180/pi)*(atan((sqrt((Ax_f*Ax_f)+(Ay_f*Ay_f)))/Az_f)) - 3.93;
213468891 0:1221112820f7 118
213468891 0:1221112820f7 119 // Convert gyroscope readings into degrees/s
213468891 0:1221112820f7 120 Gx_f = Gx_f / 131.0;
213468891 0:1221112820f7 121 Gy_f = Gy_f / 131.0;
213468891 0:1221112820f7 122 Gz_f = Gz_f / 131.0;
213468891 0:1221112820f7 123
213468891 0:1221112820f7 124 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 125
213468891 0:1221112820f7 126 }
213468891 0:1221112820f7 127 }
213468891 0:1221112820f7 128