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@4:29d8fa241626, 2016-04-15 (annotated)

Committer:
zork
Date:
Fri Apr 15 11:27:25 2016 +0000
Revision:
4:29d8fa241626
Parent:
3:b3aa280de9f3 
Executable firmware. Has the chair act as a mouse over HID protocol (and wire). Mouse position correlates to absolute rotation positions of chair (cumulative of both seat-shells). Disables use of regular mouse (because of abs pos).

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