Mark van Raai / Mbed 2 deprecated LC_V1p0_MPU6050_Complementary

Executable firmware for standard 6-angle output of the Limbic Chair, for use in direct connection with Unity3D via vcomm port (black USB wire). Talks at 100Hz.

Dependencies:   MPU6050a QEI mbed

Fork of Seeed_Grove_3-Axis_Digital_Accelorometer_Example by Seeed

main.cpp@3:eb0fd38eb7d2, 2016-04-15 (annotated)

Committer:
zork
Date:
Fri Apr 15 11:34:20 2016 +0000
Revision:
3:eb0fd38eb7d2
Parent:
2:1ebd4e9e4218 
v1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedAustin 0:6f0e33b2eb6e 1
mbedAustin 0:6f0e33b2eb6e 2 #include <mbed.h>
zork 2:1ebd4e9e4218 3 #include "QEI.h"
zork 2:1ebd4e9e4218 4 #include "MPU6050.h"
zork 2:1ebd4e9e4218 5 #include <Timer.h>
zork 2:1ebd4e9e4218 6
zork 2:1ebd4e9e4218 7 MPU6050 MPUR(p9, p10);
zork 2:1ebd4e9e4218 8 MPU6050 MPUL(p28, p27);
zork 2:1ebd4e9e4218 9
zork 2:1ebd4e9e4218 10 Serial pc(USBTX, USBRX); // tx, rx
zork 2:1ebd4e9e4218 11
zork 2:1ebd4e9e4218 12 const float M_PI = 3.14156;
zork 2:1ebd4e9e4218 13 const float ACC_FACTOR = 2.0/32767.0;
zork 2:1ebd4e9e4218 14 const float GYR_FACTOR = (250.0/32767.0);
zork 2:1ebd4e9e4218 15 // Default alpha, for non-dynamic use
zork 2:1ebd4e9e4218 16 const float ALPHA = 0.03;
zork 2:1ebd4e9e4218 17 // Gyro vector magnitude thresholds for dynamic alpha calculation
zork 2:1ebd4e9e4218 18 // With Gyro sens of +/- 250, the max magnitude is slightly over 433
zork 2:1ebd4e9e4218 19 const float MAGN_MIN = 5.0;
zork 2:1ebd4e9e4218 20 const float MAGN_MAX = 10.0;
zork 2:1ebd4e9e4218 21 const float ALPHA_MIN = 0.02;
zork 2:1ebd4e9e4218 22 const float ALPHA_MAX = 0.1;
zork 2:1ebd4e9e4218 23
zork 2:1ebd4e9e4218 24 float alpha[2];
zork 2:1ebd4e9e4218 25 const float DT = 0.01;
zork 2:1ebd4e9e4218 26 #define LEFT 0
zork 2:1ebd4e9e4218 27 #define RIGHT 1
zork 2:1ebd4e9e4218 28
zork 2:1ebd4e9e4218 29 int accSmpCount[2];
zork 2:1ebd4e9e4218 30
zork 2:1ebd4e9e4218 31 float Acc[2][3];
zork 2:1ebd4e9e4218 32 float Gyr[2][3];
zork 2:1ebd4e9e4218 33
zork 2:1ebd4e9e4218 34 float pitchL, yawL, rollL, pitchR, yawR, rollR;
zork 2:1ebd4e9e4218 35 float pitchAcc[2], rollAcc[2];
zork 2:1ebd4e9e4218 36
zork 2:1ebd4e9e4218 37 QEI encL (p5, p6, NC, 5000);
zork 2:1ebd4e9e4218 38 QEI encR (p7, p8, NC, 5000);
zork 2:1ebd4e9e4218 39
zork 2:1ebd4e9e4218 40 //Timer lowerResTimer; // Software-wise lowers resolution of the encoder, for testing
zork 2:1ebd4e9e4218 41 //float atten = 1.0;
zork 2:1ebd4e9e4218 42 DigitalOut myLed1(LED1);
zork 2:1ebd4e9e4218 43
zork 2:1ebd4e9e4218 44 void GetEncoderAngles(){
zork 2:1ebd4e9e4218 45 /*lowerResTimer.start();
zork 2:1ebd4e9e4218 46 // 40s which is 8 1.8
zork 2:1ebd4e9e4218 47 if (lowerResTimer.read_ms() > 5000.0){
zork 2:1ebd4e9e4218 48 atten += 0.1;
zork 2:1ebd4e9e4218 49 lowerResTimer.reset();
zork 2:1ebd4e9e4218 50 }*/
zork 2:1ebd4e9e4218 51 float atten = 5.0;
zork 2:1ebd4e9e4218 52 yawL = floor((float)encL.getPulses()/atten)*180.0/(5000.0/atten);
zork 2:1ebd4e9e4218 53 yawR = floor((float)encR.getPulses()/atten)*180.0/(5000.0/atten);
zork 2:1ebd4e9e4218 54 }
zork 2:1ebd4e9e4218 55
zork 2:1ebd4e9e4218 56 void GetIMUAnglesAvg(char sensor){
zork 2:1ebd4e9e4218 57 //Low Pass Filter
zork 2:1ebd4e9e4218 58 if (sensor == LEFT){
zork 2:1ebd4e9e4218 59 Acc[LEFT][0] += (double)MPUL.getAcceleroRawX()*ACC_FACTOR * 0.33;
zork 2:1ebd4e9e4218 60 Acc[LEFT][1] += (double)MPUL.getAcceleroRawY()*ACC_FACTOR * 0.33;
zork 2:1ebd4e9e4218 61 Acc[LEFT][2] += (double)MPUL.getAcceleroRawZ()*ACC_FACTOR * 0.33;
zork 2:1ebd4e9e4218 62 Gyr[LEFT][0] += (double)MPUL.getGyroRawX()*GYR_FACTOR * 0.33;
zork 2:1ebd4e9e4218 63 Gyr[LEFT][1] += (double)MPUL.getGyroRawY()*GYR_FACTOR * 0.33;
zork 2:1ebd4e9e4218 64 Gyr[LEFT][2] += (double)MPUL.getGyroRawZ()*GYR_FACTOR * 0.33;
zork 2:1ebd4e9e4218 65 }
zork 2:1ebd4e9e4218 66 //Low Pass Filter
zork 2:1ebd4e9e4218 67 if (sensor == RIGHT){
zork 2:1ebd4e9e4218 68 Acc[RIGHT][0] += (double)MPUR.getAcceleroRawX()*ACC_FACTOR * 0.33;
zork 2:1ebd4e9e4218 69 Acc[RIGHT][1] += (double)MPUR.getAcceleroRawY()*ACC_FACTOR * 0.33;
zork 2:1ebd4e9e4218 70 Acc[RIGHT][2] += (double)MPUR.getAcceleroRawZ()*ACC_FACTOR * 0.33;
zork 2:1ebd4e9e4218 71 Gyr[RIGHT][0] += (double)MPUR.getGyroRawX()*GYR_FACTOR * 0.33;
zork 2:1ebd4e9e4218 72 Gyr[RIGHT][1] += (double)MPUR.getGyroRawY()*GYR_FACTOR * 0.33;
zork 2:1ebd4e9e4218 73 Gyr[RIGHT][2] += (double)MPUR.getGyroRawZ()*GYR_FACTOR * 0.33;
zork 2:1ebd4e9e4218 74 }
zork 2:1ebd4e9e4218 75 }
zork 2:1ebd4e9e4218 76
zork 2:1ebd4e9e4218 77 void Purge(){
zork 2:1ebd4e9e4218 78 for (int i = 0; i < 2; i++){
zork 2:1ebd4e9e4218 79 for (int j = 0; j < 3; j++){
zork 2:1ebd4e9e4218 80 Acc[i][j] = 0;
zork 2:1ebd4e9e4218 81 Gyr[i][j] = 0;
zork 2:1ebd4e9e4218 82 }
zork 2:1ebd4e9e4218 83 }
zork 2:1ebd4e9e4218 84 }
mbedAustin 0:6f0e33b2eb6e 85
zork 2:1ebd4e9e4218 86 void CalcAccAngles(){
zork 2:1ebd4e9e4218 87 //Roll & Pitch Equations
zork 2:1ebd4e9e4218 88 rollAcc[LEFT] = (atan2(-Acc[LEFT][1], Acc[LEFT][2])*180.0)/M_PI;
zork 2:1ebd4e9e4218 89 pitchAcc[LEFT] = (atan2(Acc[LEFT][0], sqrt(Acc[LEFT][1]*Acc[LEFT][1] + Acc[LEFT][2]*Acc[LEFT][2]))*180.0)/M_PI;
zork 2:1ebd4e9e4218 90 //Roll & Pitch Equations
zork 2:1ebd4e9e4218 91 rollAcc[RIGHT] = (atan2(-Acc[RIGHT][1], Acc[RIGHT][2])*180.0)/M_PI;
zork 2:1ebd4e9e4218 92 pitchAcc[RIGHT] = (atan2(Acc[RIGHT][0], sqrt(Acc[RIGHT][1]*Acc[RIGHT][1] + Acc[RIGHT][2]*Acc[RIGHT][2]))*180.0)/M_PI;
zork 2:1ebd4e9e4218 93 }
zork 2:1ebd4e9e4218 94
zork 2:1ebd4e9e4218 95 void SendPrintDebug(){
zork 2:1ebd4e9e4218 96 pc.printf("PitchL=%6.2f, YawL=%6.2f, RollL=%6.2f, PitchR=%6.2f, YawR=%6.2f, RollR=%6.2f\n\r",pitchL,yawL,rollL,pitchR,yawR,rollR);
zork 2:1ebd4e9e4218 97 }
zork 2:1ebd4e9e4218 98
zork 2:1ebd4e9e4218 99 void SendBytes(){
zork 2:1ebd4e9e4218 100 short Lx, Ly, Lz, Rx, Ry, Rz;
zork 2:1ebd4e9e4218 101 char buffer[16];
zork 2:1ebd4e9e4218 102
zork 2:1ebd4e9e4218 103 Lx = (short) (pitchL * 32767.0/180.0);
zork 2:1ebd4e9e4218 104 Ly = (short) (yawL * 32767.0/180.0);
zork 2:1ebd4e9e4218 105 Lz = (short) ((rollL) * 32767.0/180.0);
zork 2:1ebd4e9e4218 106
zork 2:1ebd4e9e4218 107 Rx = (short) (pitchR * 32767.0/180.0);
zork 2:1ebd4e9e4218 108 Ry = (short) (yawR * 32767.0/180.0);
zork 2:1ebd4e9e4218 109 Rz = (short) ((rollR) * 32767.0/180.0);
zork 2:1ebd4e9e4218 110
zork 2:1ebd4e9e4218 111 buffer[0] = '#';
zork 2:1ebd4e9e4218 112 buffer[1] = '!';
zork 2:1ebd4e9e4218 113 buffer[3] = (char) (Lx >> 8 & 0xFF);
zork 2:1ebd4e9e4218 114 buffer[2] = (char) (Lx & 0xFF);
zork 2:1ebd4e9e4218 115 buffer[5] = (char) (Ly >> 8 & 0xFF);
zork 2:1ebd4e9e4218 116 buffer[4] = (char) (Ly & 0xFF);
zork 2:1ebd4e9e4218 117 buffer[7] = (char) (Lz >> 8 & 0xFF);
zork 2:1ebd4e9e4218 118 buffer[6] = (char) (Lz & 0xFF);
zork 2:1ebd4e9e4218 119 buffer[9] = (char) (Rx >> 8 & 0xFF);
zork 2:1ebd4e9e4218 120 buffer[8] = (char) (Rx & 0xFF);
zork 2:1ebd4e9e4218 121 buffer[11] = (char) (Ry >> 8 & 0xFF);
zork 2:1ebd4e9e4218 122 buffer[10] = (char) (Ry & 0xFF);
zork 2:1ebd4e9e4218 123 buffer[13] = (char) (Rz >> 8 & 0xFF);
zork 2:1ebd4e9e4218 124 buffer[12] = (char) (Rz & 0xFF);
zork 2:1ebd4e9e4218 125
zork 2:1ebd4e9e4218 126 buffer[14] = '~';
zork 2:1ebd4e9e4218 127 buffer[15] = '~';
zork 2:1ebd4e9e4218 128 pc.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",buffer[0],buffer[1],buffer[2],buffer[3],buffer[4],buffer[5],buffer[6],buffer[7],buffer[8],buffer[9],buffer[10],buffer[11],buffer[12],buffer[13],buffer[14],buffer[15]);
zork 2:1ebd4e9e4218 129 //pc.write(buffer, sizeof(buffer));
zork 2:1ebd4e9e4218 130 }
zork 2:1ebd4e9e4218 131
zork 2:1ebd4e9e4218 132 void Complementary () {
zork 2:1ebd4e9e4218 133 // a=tau / (tau + loop time)
zork 2:1ebd4e9e4218 134 // newAngle = angle measured with atan2 using the accelerometer
zork 2:1ebd4e9e4218 135 // newRate = angle measured using the gyro
zork 2:1ebd4e9e4218 136 // dt = loop time in seconds
zork 2:1ebd4e9e4218 137
zork 2:1ebd4e9e4218 138 // Integrate the gyroscope data -> int(angularSpeed) = angle
zork 2:1ebd4e9e4218 139 // Only integrate if faster than 0.075 degrees/s, to reduce drift
zork 2:1ebd4e9e4218 140 // No active person moves that slow, I guess ... This is most important
zork 2:1ebd4e9e4218 141 // for Yaw (i.e. gyro[2] since that angle cannot be compensated for using acc.)
zork 2:1ebd4e9e4218 142 //if (fabs(gyro[2] * dt) > 0.075)
zork 2:1ebd4e9e4218 143 pitchL += Gyr[0][1] * DT; // Angle around the X-axis, roll
zork 2:1ebd4e9e4218 144 rollL += Gyr[0][0] * DT; // Angle around the Z-axis, pitch
zork 2:1ebd4e9e4218 145
zork 2:1ebd4e9e4218 146 pitchR += Gyr[1][1] * DT; // Angle around the X-axis, roll
zork 2:1ebd4e9e4218 147 rollR -= Gyr[1][0] * DT; // Angle around the Z-axis, pitch
zork 2:1ebd4e9e4218 148
zork 2:1ebd4e9e4218 149 // Compensate for drift with accelerometer data if !bullshit
zork 2:1ebd4e9e4218 150 // Sensitivity = -16 to 16 G at 16Bit -> 16G = 32768 && 0.5G = 1024
zork 2:1ebd4e9e4218 151 pitchL += alpha[LEFT] * (pitchAcc[LEFT] - pitchL);
zork 2:1ebd4e9e4218 152 rollL += alpha[LEFT] * (rollAcc[LEFT] - rollL);
zork 2:1ebd4e9e4218 153
zork 2:1ebd4e9e4218 154 pitchR += alpha[RIGHT] * (pitchAcc[RIGHT] - pitchR);
zork 2:1ebd4e9e4218 155 rollR += alpha[RIGHT] * (rollAcc[RIGHT] - rollR);
zork 2:1ebd4e9e4218 156
zork 2:1ebd4e9e4218 157 // Override that fucking shit angle, we don't use it now anyway grrrr
zork 2:1ebd4e9e4218 158 rollL = rollR = 0;
zork 2:1ebd4e9e4218 159 }
zork 2:1ebd4e9e4218 160
zork 2:1ebd4e9e4218 161 void GetDynamicAlpha(int Sensor){
zork 2:1ebd4e9e4218 162 float magn;
zork 2:1ebd4e9e4218 163 magn = sqrt(pow(Gyr[Sensor][0],2)+pow(Gyr[Sensor][1],2)+pow(Gyr[Sensor][2],2));
zork 2:1ebd4e9e4218 164 if (magn > MAGN_MIN && magn < MAGN_MAX) alpha[Sensor] = ALPHA_MAX * (magn / MAGN_MAX);
zork 2:1ebd4e9e4218 165 else if (magn > MAGN_MAX) alpha[Sensor] = ALPHA_MAX;
zork 2:1ebd4e9e4218 166 else alpha[Sensor] = ALPHA_MIN;
zork 2:1ebd4e9e4218 167 }
mbedAustin 0:6f0e33b2eb6e 168
mbedAustin 0:6f0e33b2eb6e 169 int main()
mbedAustin 0:6f0e33b2eb6e 170 {
zork 2:1ebd4e9e4218 171 myLed1=1;
zork 2:1ebd4e9e4218 172 Timer sendtimer, updateAccTimer;
zork 2:1ebd4e9e4218 173 pc.baud(115200);
zork 2:1ebd4e9e4218 174 MPUL.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
zork 2:1ebd4e9e4218 175 MPUR.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
zork 2:1ebd4e9e4218 176 MPUL.setGyroRange(MPU6050_GYRO_RANGE_250);
zork 2:1ebd4e9e4218 177 MPUR.setGyroRange(MPU6050_GYRO_RANGE_250);
zork 2:1ebd4e9e4218 178
zork 2:1ebd4e9e4218 179 pitchL = rollL = pitchR = rollR = 0;
zork 2:1ebd4e9e4218 180 alpha[0] = alpha[1] = ALPHA;
zork 2:1ebd4e9e4218 181
zork 2:1ebd4e9e4218 182 sendtimer.start();
mbedAustin 0:6f0e33b2eb6e 183 while(1){
zork 2:1ebd4e9e4218 184 if(MPUL.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt
zork 2:1ebd4e9e4218 185 GetIMUAnglesAvg(LEFT);
zork 2:1ebd4e9e4218 186 accSmpCount[LEFT]++;
zork 2:1ebd4e9e4218 187 }
zork 2:1ebd4e9e4218 188 if(MPUR.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt
zork 2:1ebd4e9e4218 189 GetIMUAnglesAvg(RIGHT);
zork 2:1ebd4e9e4218 190 accSmpCount[RIGHT]++;
zork 2:1ebd4e9e4218 191 }
zork 2:1ebd4e9e4218 192
zork 2:1ebd4e9e4218 193 if (sendtimer.read_ms () >= 10){
zork 2:1ebd4e9e4218 194 // Dynamic alpha based on amount of motion
zork 2:1ebd4e9e4218 195 GetDynamicAlpha(LEFT);
zork 2:1ebd4e9e4218 196 GetDynamicAlpha(RIGHT);
zork 2:1ebd4e9e4218 197
zork 2:1ebd4e9e4218 198 //pc.printf("%d %d \n\r", accSmpCount[LEFT], accSmpCount[RIGHT]);
zork 2:1ebd4e9e4218 199 //pc.printf("%f6.6 %f6.6 \n\r", Gyr[LEFT][0], Gyr[LEFT][2]);
zork 2:1ebd4e9e4218 200 GetEncoderAngles();
zork 2:1ebd4e9e4218 201 CalcAccAngles();
zork 2:1ebd4e9e4218 202 Complementary ();
zork 2:1ebd4e9e4218 203 SendBytes();
zork 2:1ebd4e9e4218 204 //SendPrintDebug();
zork 2:1ebd4e9e4218 205 sendtimer.reset();
zork 2:1ebd4e9e4218 206 accSmpCount[LEFT] = accSmpCount[RIGHT] = 0;
zork 2:1ebd4e9e4218 207 // Purge
zork 2:1ebd4e9e4218 208 Purge();
zork 2:1ebd4e9e4218 209 }
mbedAustin 0:6f0e33b2eb6e 210
zork 2:1ebd4e9e4218 211 //pc.printf("1 of X=%2.2fg, Y=%2.2fg, Z=%2.2fg",MMA1.x(),MMA1.y(),MMA1.z());
zork 2:1ebd4e9e4218 212 //pc.printf(" 2 of X=%2.2fg, Y=%2.2fg, Z=%2.2fg\n\r",MMA2.x(),MMA2.y(),MMA2.z());
zork 2:1ebd4e9e4218 213 //wait(.01);
mbedAustin 0:6f0e33b2eb6e 214 }
mbedAustin 0:6f0e33b2eb6e 215 }