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
Diff: main.cpp
- Revision:
- 2:a7dc264ecd98
- Parent:
- 1:94e29063fb0d
- Child:
- 3:b3aa280de9f3
--- a/main.cpp Sat Sep 06 00:47:38 2014 +0000 +++ b/main.cpp Tue Apr 05 16:19:38 2016 +0000 @@ -1,20 +1,249 @@ #include <mbed.h> -#include "MMA7660.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 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 = 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; + } +} -MMA7660 accelemeter; +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() { - int8_t x, y, z; - float ax,ay,az; - accelemeter.init(); + 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; + + alpha[0] = alpha[1] = ALPHA; + + sendtimer.start(); + + int x,y; while(1){ - accelemeter.getXYZ(&x,&y,&z); - printf("X=%d, Y=%d, Z=%d, ",x,y,z); + 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; + } + + int x = -((yawL-yawL0))*960 + 960; + int y = ((pitchL-pitchL0)*2)*1080; + + if (x < 0) x = 0; + if (x > 1920) x = 1920; + if (y < 0) y = 0; + if (y > 1080) y = 1080; + + /*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(); + } - accelemeter.getAcceleration(&ax,&ay,&az); - printf("Accleration of X=%2.2fg, Y=%2.2fg, Z=%2.2fg\n\r",ax,ay,az); - wait(.5); + //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); } } \ No newline at end of file