Has the Limbic Controller act as a mouse over HID protocol (and wire). Generates absolute position from seat rotation (both shells cumulative), as such disables use of regular mouse.
Dependencies: MPU6050 QEI USBDevice mbed
Fork of Seeed_Grove_3-Axis_Digital_Accelorometer_Example by
main.cpp
- Committer:
- zork
- Date:
- 2016-04-15
- Revision:
- 4:29d8fa241626
- Parent:
- 3:b3aa280de9f3
File content as of revision 4:29d8fa241626:
#include <mbed.h> #include "QEI.h" #include "MPU6050.h" #include <Timer.h> #include "USBMouse.h" USBMouse mouse; 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 pitchL0, yawL0, rollL0, pitchR0, yawR0, rollR0; float pitchLold, yawLold, pitchRold, yawRold; 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; void GetEncoderAngles(){ /*lowerResTimer.start(); // 40s which is 8 1.8 if (lowerResTimer.read_ms() > 5000.0){ atten += 0.1; lowerResTimer.reset(); }*/ float atten = 1.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() { 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; bool firstFrame = true; pitchL0 = yawL0 = pitchR0 = yawR0 = 0; pitchLold = yawLold = pitchRold = yawRold = 0; alpha[0] = alpha[1] = ALPHA; sendtimer.start(); double x,y; 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]); //SendPrintDebug(); GetEncoderAngles(); CalcAccAngles(); Complementary (); //SendBytes(); if (firstFrame){ firstFrame = false; pitchL0 = pitchL; yawL0 = yawL; pitchR0 = pitchR; yawR0 = yawR; } float accellYaw; if ((abs(yawR - yawRold) + abs(yawL - yawLold)) > 10.0) accellYaw = 100.0; else if ((abs(yawR - yawRold) + abs(yawL - yawLold)) < 0.1) accellYaw = 0.01; else accellYaw = 0.5F; yawRold = yawR; yawLold = yawL; pitchLold = pitchL; pitchRold = pitchR; x = ((yawR-yawR0)+(yawL-yawL0))*0.075*-16384.0 + 16384; y = ((pitchL-pitchL0)+(pitchR-pitchR0))*0.15*16384.0 - 16384; if (x < 0) x = 0; if (x > 32767) x = 32767; if (y < 0) y = 0; if (y > 32767) y = 32767; mouse.move((int)x,(int)y); /*if (((yawL-yawL0)) > 2.5){ mouse.press(1); mouse.release(2); } else if (((yawL-yawL0)) < -2.5){ mouse.press(2); mouse.release(1); } else { mouse.release(1); mouse.release(2); }*/ 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); } }