Quidditch-Embedded-System/main.cpp at master · quidditch-simulator/Quidditch-Embedded-System · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

#include "mbed.h"
#include "MPU6050.h"
#include <math.h>
#define pi 3.141592654
Serial bluetooth(D10, D2);//Tx, Rx
MPU6050 AccGyro(D14, D15); // Create an MPU object called AccGyro

Serial pc(SERIAL_TX, SERIAL_RX);    //To use the PC as a console (display output)

int16_t Ax, Ay, Az, Gx, Gy, Gz;
float Ax_f, Ay_f, Az_f;
double Gx_f, Gy_f, Gz_f;
float Ax_f_sum, Ay_f_sum, Az_f_sum, Gx_f_sum, Gy_f_sum, Gz_f_sum;
float roll, pitch, yaw;

int main() {

    uint16_t AccelReadings[3] = {0, 0, 0};
    uint16_t GyroReadings[3] = {0, 0, 0};
    uint8_t DevId;

    pc.printf("Starting MPU6050 test...\n");
    DevId = AccGyro.getWhoAmI();

    if(DevId == 0x68){
        pc.printf("\n");
        pc.printf("MPU6050 detected...\n");
        pc.printf("Device ID is: 0x%02x\n", DevId);
        pc.printf("\n");
    }else{
        pc.printf("\n");
        pc.printf("MPU6050 not found...\n");
        while(1);
    }

    // The device will come up in sleep mode upon power-up.
    AccGyro.setPowerCtl_1(0x00, 0x00, 0x00, 0x00, INT_8MHz_OSC);    // Disable sleep mode
    wait(.001);

    // Full scale, +/-2000°/s, 16.4LSB°/s.
    AccGyro.setGyroConfig(GYRO_ST_OFF, GFS_2000dps); // Accelerometer elf-test trigger off.
    wait(.001);

    // Full scale, +/-16g, 2048LSB/g.
    AccGyro.setAccelConfig(ACC_ST_OFF, AFS_16g);    // Gyroscope self-test trigger off.
    wait(.001);

    while (true) {

       wait(0.2);

       Ax_f_sum = 0;
       Ay_f_sum = 0;
       Az_f_sum = 0;
       Gx_f_sum = 0;
       Gy_f_sum = 0;
       Gz_f_sum = 0;

       for(int i = 0; i < 10; i = i + 1)    // Take ten analog input readings
       {
            AccGyro.readAccel(AccelReadings);   // Extract accelerometer measurements
            AccGyro.readGyro(GyroReadings);     // Extract gyroscope measurements

            // 2s complement acclerometer and gyroscope values
            Ax = AccelReadings[0];
            Ay = AccelReadings[1];
            Az = AccelReadings[2];
            Gx = GyroReadings[0];
            Gy = GyroReadings[1];
            Gz = GyroReadings[2];

            // Add every reading to the sum variables
            Ax_f_sum = Ax_f_sum + (float)Ax;
            Ay_f_sum = Ay_f_sum + (float)Ay;
            Az_f_sum = Az_f_sum + (float)Az;
            Gx_f_sum = Gx_f_sum + (float)Gx;
            Gy_f_sum = Gy_f_sum + (float)Gy;
            Gz_f_sum = Gz_f_sum + (float)Gz;
        }

        // Divide by 10 to get the averaged value
        Ax_f = Ax_f_sum / 10;
        Ay_f = Ay_f_sum / 10;
        Az_f = Az_f_sum / 10;
        Gx_f = Gx_f_sum / 10;
        Gy_f = Gy_f_sum / 10;
        Gz_f = Gz_f_sum / 10;

        // 1. Calculate actual roll, pitch and yaw angles in degrees
        // 2. Calibrate readings by adding or substracting the off-set
        roll = (180/pi)*(atan(Ax_f/(sqrt((Ay_f*Ay_f)+(Az_f*Az_f))))) - 4.36;
        pitch = (180/pi)*(atan(Ay_f/(sqrt((Ax_f*Ax_f)+(Az_f*Az_f))))) - 0.063;
        yaw = (180/pi)*(atan((sqrt((Ax_f*Ax_f)+(Ay_f*Ay_f)))/Az_f)) - 3.93;

        // Convert gyroscope readings into degrees/s
        Gx_f = Gx_f / 131.0;
        Gy_f = Gy_f / 131.0;
        Gz_f = Gz_f / 131.0;
        pc.printf("Gyro(deg/s) X: %.3f Y: %.3f Z: %.3f || Accel(deg) Roll: %.3f, Pitch: %.3f, Yaw: %.3f \r\n", Gx_f, Gy_f, Gz_f, roll, pitch, yaw);
        bluetooth.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);

    }
}