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
main.cpp
- Committer:
- zork
- Date:
- 2016-04-15
- Revision:
- 3:eb0fd38eb7d2
- Parent:
- 2:1ebd4e9e4218
File content as of revision 3:eb0fd38eb7d2:
#include <mbed.h> #include "QEI.h" #include "MPU6050.h" #include <Timer.h> MPU6050 MPUR(p9, p10); MPU6050 MPUL(p28, p27); Serial pc(USBTX, USBRX); // tx, rx const float M_PI = 3.14156; const float ACC_FACTOR = 2.0/32767.0; const float GYR_FACTOR = (250.0/32767.0); // Default alpha, for non-dynamic use const float ALPHA = 0.03; // Gyro vector magnitude thresholds for dynamic alpha calculation // With Gyro sens of +/- 250, the max magnitude is slightly over 433 const float MAGN_MIN = 5.0; const float MAGN_MAX = 10.0; const float ALPHA_MIN = 0.02; const float ALPHA_MAX = 0.1; float alpha[2]; const float DT = 0.01; #define LEFT 0 #define RIGHT 1 int accSmpCount[2]; float Acc[2][3]; float Gyr[2][3]; float pitchL, yawL, rollL, pitchR, yawR, rollR; float pitchAcc[2], rollAcc[2]; QEI encL (p5, p6, NC, 5000); QEI encR (p7, p8, NC, 5000); //Timer lowerResTimer; // Software-wise lowers resolution of the encoder, for testing //float atten = 1.0; DigitalOut myLed1(LED1); void GetEncoderAngles(){ /*lowerResTimer.start(); // 40s which is 8 1.8 if (lowerResTimer.read_ms() > 5000.0){ atten += 0.1; lowerResTimer.reset(); }*/ float atten = 5.0; yawL = floor((float)encL.getPulses()/atten)*180.0/(5000.0/atten); yawR = floor((float)encR.getPulses()/atten)*180.0/(5000.0/atten); } void GetIMUAnglesAvg(char sensor){ //Low Pass Filter if (sensor == LEFT){ Acc[LEFT][0] += (double)MPUL.getAcceleroRawX()*ACC_FACTOR * 0.33; Acc[LEFT][1] += (double)MPUL.getAcceleroRawY()*ACC_FACTOR * 0.33; Acc[LEFT][2] += (double)MPUL.getAcceleroRawZ()*ACC_FACTOR * 0.33; Gyr[LEFT][0] += (double)MPUL.getGyroRawX()*GYR_FACTOR * 0.33; Gyr[LEFT][1] += (double)MPUL.getGyroRawY()*GYR_FACTOR * 0.33; Gyr[LEFT][2] += (double)MPUL.getGyroRawZ()*GYR_FACTOR * 0.33; } //Low Pass Filter if (sensor == RIGHT){ Acc[RIGHT][0] += (double)MPUR.getAcceleroRawX()*ACC_FACTOR * 0.33; Acc[RIGHT][1] += (double)MPUR.getAcceleroRawY()*ACC_FACTOR * 0.33; Acc[RIGHT][2] += (double)MPUR.getAcceleroRawZ()*ACC_FACTOR * 0.33; Gyr[RIGHT][0] += (double)MPUR.getGyroRawX()*GYR_FACTOR * 0.33; Gyr[RIGHT][1] += (double)MPUR.getGyroRawY()*GYR_FACTOR * 0.33; Gyr[RIGHT][2] += (double)MPUR.getGyroRawZ()*GYR_FACTOR * 0.33; } } void Purge(){ for (int i = 0; i < 2; i++){ for (int j = 0; j < 3; j++){ Acc[i][j] = 0; Gyr[i][j] = 0; } } } void CalcAccAngles(){ //Roll & Pitch Equations rollAcc[LEFT] = (atan2(-Acc[LEFT][1], Acc[LEFT][2])*180.0)/M_PI; pitchAcc[LEFT] = (atan2(Acc[LEFT][0], sqrt(Acc[LEFT][1]*Acc[LEFT][1] + Acc[LEFT][2]*Acc[LEFT][2]))*180.0)/M_PI; //Roll & Pitch Equations rollAcc[RIGHT] = (atan2(-Acc[RIGHT][1], Acc[RIGHT][2])*180.0)/M_PI; pitchAcc[RIGHT] = (atan2(Acc[RIGHT][0], sqrt(Acc[RIGHT][1]*Acc[RIGHT][1] + Acc[RIGHT][2]*Acc[RIGHT][2]))*180.0)/M_PI; } void SendPrintDebug(){ 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); } void SendBytes(){ short Lx, Ly, Lz, Rx, Ry, Rz; char buffer[16]; Lx = (short) (pitchL * 32767.0/180.0); Ly = (short) (yawL * 32767.0/180.0); Lz = (short) ((rollL) * 32767.0/180.0); Rx = (short) (pitchR * 32767.0/180.0); Ry = (short) (yawR * 32767.0/180.0); Rz = (short) ((rollR) * 32767.0/180.0); buffer[0] = '#'; buffer[1] = '!'; buffer[3] = (char) (Lx >> 8 & 0xFF); buffer[2] = (char) (Lx & 0xFF); buffer[5] = (char) (Ly >> 8 & 0xFF); buffer[4] = (char) (Ly & 0xFF); buffer[7] = (char) (Lz >> 8 & 0xFF); buffer[6] = (char) (Lz & 0xFF); buffer[9] = (char) (Rx >> 8 & 0xFF); buffer[8] = (char) (Rx & 0xFF); buffer[11] = (char) (Ry >> 8 & 0xFF); buffer[10] = (char) (Ry & 0xFF); buffer[13] = (char) (Rz >> 8 & 0xFF); buffer[12] = (char) (Rz & 0xFF); buffer[14] = '~'; buffer[15] = '~'; 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]); //pc.write(buffer, sizeof(buffer)); } void Complementary () { // a=tau / (tau + loop time) // newAngle = angle measured with atan2 using the accelerometer // newRate = angle measured using the gyro // dt = loop time in seconds // Integrate the gyroscope data -> int(angularSpeed) = angle // Only integrate if faster than 0.075 degrees/s, to reduce drift // No active person moves that slow, I guess ... This is most important // for Yaw (i.e. gyro[2] since that angle cannot be compensated for using acc.) //if (fabs(gyro[2] * dt) > 0.075) pitchL += Gyr[0][1] * DT; // Angle around the X-axis, roll rollL += Gyr[0][0] * DT; // Angle around the Z-axis, pitch pitchR += Gyr[1][1] * DT; // Angle around the X-axis, roll rollR -= Gyr[1][0] * DT; // Angle around the Z-axis, pitch // Compensate for drift with accelerometer data if !bullshit // Sensitivity = -16 to 16 G at 16Bit -> 16G = 32768 && 0.5G = 1024 pitchL += alpha[LEFT] * (pitchAcc[LEFT] - pitchL); rollL += alpha[LEFT] * (rollAcc[LEFT] - rollL); pitchR += alpha[RIGHT] * (pitchAcc[RIGHT] - pitchR); rollR += alpha[RIGHT] * (rollAcc[RIGHT] - rollR); // Override that fucking shit angle, we don't use it now anyway grrrr rollL = rollR = 0; } void GetDynamicAlpha(int Sensor){ float magn; magn = sqrt(pow(Gyr[Sensor][0],2)+pow(Gyr[Sensor][1],2)+pow(Gyr[Sensor][2],2)); if (magn > MAGN_MIN && magn < MAGN_MAX) alpha[Sensor] = ALPHA_MAX * (magn / MAGN_MAX); else if (magn > MAGN_MAX) alpha[Sensor] = ALPHA_MAX; else alpha[Sensor] = ALPHA_MIN; } int main() { myLed1=1; Timer sendtimer, updateAccTimer; pc.baud(115200); MPUL.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); MPUR.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); MPUL.setGyroRange(MPU6050_GYRO_RANGE_250); MPUR.setGyroRange(MPU6050_GYRO_RANGE_250); pitchL = rollL = pitchR = rollR = 0; alpha[0] = alpha[1] = ALPHA; sendtimer.start(); while(1){ if(MPUL.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt GetIMUAnglesAvg(LEFT); accSmpCount[LEFT]++; } if(MPUR.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt GetIMUAnglesAvg(RIGHT); accSmpCount[RIGHT]++; } if (sendtimer.read_ms () >= 10){ // Dynamic alpha based on amount of motion GetDynamicAlpha(LEFT); GetDynamicAlpha(RIGHT); //pc.printf("%d %d \n\r", accSmpCount[LEFT], accSmpCount[RIGHT]); //pc.printf("%f6.6 %f6.6 \n\r", Gyr[LEFT][0], Gyr[LEFT][2]); GetEncoderAngles(); CalcAccAngles(); Complementary (); SendBytes(); //SendPrintDebug(); sendtimer.reset(); accSmpCount[LEFT] = accSmpCount[RIGHT] = 0; // Purge Purge(); } //pc.printf("1 of X=%2.2fg, Y=%2.2fg, Z=%2.2fg",MMA1.x(),MMA1.y(),MMA1.z()); //pc.printf(" 2 of X=%2.2fg, Y=%2.2fg, Z=%2.2fg\n\r",MMA2.x(),MMA2.y(),MMA2.z()); //wait(.01); } }