Mark van Raai / Mbed 2 deprecated LC_V1p1_Mouse

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 Seeed

main.cpp@2:a7dc264ecd98, 2016-04-05 (annotated)

Committer:
zork
Date:
Tue Apr 05 16:19:38 2016 +0000
Revision:
2:a7dc264ecd98
Parent:
1:94e29063fb0d
Child:
3:b3aa280de9f3
sdfj,ghsdrklgj

Who changed what in which revision?

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