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@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?
User | Revision | Line number | New 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 | } |