Code for the sword component of Clash!, a motion controlled pair of input devices for the game Chivalry: Medieval Warfare.
Dependencies: DebounceIn L3GD20 MMA8451Q USBDevice mbed
main.cpp@0:a7b02bcbe5fc, 2014-09-29 (annotated)
- Committer:
- ddrew73
- Date:
- Mon Sep 29 01:52:45 2014 +0000
- Revision:
- 0:a7b02bcbe5fc
Nothin.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ddrew73 | 0:a7b02bcbe5fc | 1 | /* |
ddrew73 | 0:a7b02bcbe5fc | 2 | CS294 Interactive Device Design - Homework 3 |
ddrew73 | 0:a7b02bcbe5fc | 3 | CLASH! Sword |
ddrew73 | 0:a7b02bcbe5fc | 4 | |
ddrew73 | 0:a7b02bcbe5fc | 5 | Daniel Drew |
ddrew73 | 0:a7b02bcbe5fc | 6 | Filip Maksimovic |
ddrew73 | 0:a7b02bcbe5fc | 7 | */ |
ddrew73 | 0:a7b02bcbe5fc | 8 | |
ddrew73 | 0:a7b02bcbe5fc | 9 | #include "mbed.h" |
ddrew73 | 0:a7b02bcbe5fc | 10 | #include "MMA8451Q.h" |
ddrew73 | 0:a7b02bcbe5fc | 11 | #include "DebounceIn.h" |
ddrew73 | 0:a7b02bcbe5fc | 12 | #include "L3GD20.h" |
ddrew73 | 0:a7b02bcbe5fc | 13 | #include "USBMouse.h" |
ddrew73 | 0:a7b02bcbe5fc | 14 | |
ddrew73 | 0:a7b02bcbe5fc | 15 | //FOR GESTURE DEBUGGING! Make sure to uncomment the object below! |
ddrew73 | 0:a7b02bcbe5fc | 16 | //#include "USBKeyboard.h" |
ddrew73 | 0:a7b02bcbe5fc | 17 | |
ddrew73 | 0:a7b02bcbe5fc | 18 | //Define I2C stuff for Accel, Gyro |
ddrew73 | 0:a7b02bcbe5fc | 19 | PinName const SDA1 = PTE25; |
ddrew73 | 0:a7b02bcbe5fc | 20 | PinName const SCL1 = PTE24; |
ddrew73 | 0:a7b02bcbe5fc | 21 | PinName const SDA2 = PTE0; |
ddrew73 | 0:a7b02bcbe5fc | 22 | PinName const SCL2 = PTE1; |
ddrew73 | 0:a7b02bcbe5fc | 23 | |
ddrew73 | 0:a7b02bcbe5fc | 24 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
ddrew73 | 0:a7b02bcbe5fc | 25 | |
ddrew73 | 0:a7b02bcbe5fc | 26 | int main() { |
ddrew73 | 0:a7b02bcbe5fc | 27 | |
ddrew73 | 0:a7b02bcbe5fc | 28 | //Object Instantiation |
ddrew73 | 0:a7b02bcbe5fc | 29 | Serial pc(USBTX, USBRX); // tx, rx |
ddrew73 | 0:a7b02bcbe5fc | 30 | USBMouse mouse; |
ddrew73 | 0:a7b02bcbe5fc | 31 | MMA8451Q acc(SDA1, SCL1, MMA8451_I2C_ADDRESS); |
ddrew73 | 0:a7b02bcbe5fc | 32 | DigitalOut myred(LED1); |
ddrew73 | 0:a7b02bcbe5fc | 33 | DigitalOut mygreen(LED2); |
ddrew73 | 0:a7b02bcbe5fc | 34 | DigitalOut myblue(LED3); |
ddrew73 | 0:a7b02bcbe5fc | 35 | L3GD20 gyro(SDA2, SCL2); |
ddrew73 | 0:a7b02bcbe5fc | 36 | //USBKeyboard key; |
ddrew73 | 0:a7b02bcbe5fc | 37 | |
ddrew73 | 0:a7b02bcbe5fc | 38 | //Variable Instantiation |
ddrew73 | 0:a7b02bcbe5fc | 39 | int i=0; |
ddrew73 | 0:a7b02bcbe5fc | 40 | int upcount = 0, totcount = 0, overready = 0, blocked = 0, struck = 0, restcount = 0, overheaded = 0; |
ddrew73 | 0:a7b02bcbe5fc | 41 | float gx_d, gy_d, gz_d; |
ddrew73 | 0:a7b02bcbe5fc | 42 | float gx, gy, gz; |
ddrew73 | 0:a7b02bcbe5fc | 43 | float gx_zero = 0.0, gy_zero = 0.0, gz_zero = 0.0; |
ddrew73 | 0:a7b02bcbe5fc | 44 | double ts = 0.1 , norm = 0; |
ddrew73 | 0:a7b02bcbe5fc | 45 | double q0 = 1, q1 = 0, q2 = 0, q3 = 0; |
ddrew73 | 0:a7b02bcbe5fc | 46 | double q0dot = 0, q1dot = 0, q2dot = 0, q3dot = 0; |
ddrew73 | 0:a7b02bcbe5fc | 47 | double theta = 0, phi = 0, psi = 0; |
ddrew73 | 0:a7b02bcbe5fc | 48 | |
ddrew73 | 0:a7b02bcbe5fc | 49 | //Begin main program loop |
ddrew73 | 0:a7b02bcbe5fc | 50 | while(1){ |
ddrew73 | 0:a7b02bcbe5fc | 51 | |
ddrew73 | 0:a7b02bcbe5fc | 52 | //Start-up gyro "calibration" - LED turns red when it is complete |
ddrew73 | 0:a7b02bcbe5fc | 53 | myred = 1; |
ddrew73 | 0:a7b02bcbe5fc | 54 | mygreen = 1; |
ddrew73 | 0:a7b02bcbe5fc | 55 | myblue = 0; |
ddrew73 | 0:a7b02bcbe5fc | 56 | while(i < 10000){ |
ddrew73 | 0:a7b02bcbe5fc | 57 | gyro.read(&gx, &gy, &gz); |
ddrew73 | 0:a7b02bcbe5fc | 58 | gx_zero = gx_zero + gx; |
ddrew73 | 0:a7b02bcbe5fc | 59 | gy_zero = gy_zero + gy; |
ddrew73 | 0:a7b02bcbe5fc | 60 | gz_zero = gz_zero + gz; |
ddrew73 | 0:a7b02bcbe5fc | 61 | i = i+1; |
ddrew73 | 0:a7b02bcbe5fc | 62 | } |
ddrew73 | 0:a7b02bcbe5fc | 63 | myred = 0; |
ddrew73 | 0:a7b02bcbe5fc | 64 | mygreen = 1; |
ddrew73 | 0:a7b02bcbe5fc | 65 | myblue = 1; |
ddrew73 | 0:a7b02bcbe5fc | 66 | |
ddrew73 | 0:a7b02bcbe5fc | 67 | //Gyro quaternion stuff |
ddrew73 | 0:a7b02bcbe5fc | 68 | gyro.read(&gx, &gy, &gz); |
ddrew73 | 0:a7b02bcbe5fc | 69 | gx_d = gx - (gx_zero / 10000 ); |
ddrew73 | 0:a7b02bcbe5fc | 70 | gy_d = gy - (gy_zero / 10000 ); |
ddrew73 | 0:a7b02bcbe5fc | 71 | gz_d = gz - (gz_zero / 10000 ); |
ddrew73 | 0:a7b02bcbe5fc | 72 | |
ddrew73 | 0:a7b02bcbe5fc | 73 | gx_d = (double)gx_d; |
ddrew73 | 0:a7b02bcbe5fc | 74 | gy_d = (double)gy_d; |
ddrew73 | 0:a7b02bcbe5fc | 75 | gz_d = (double)gz_d; |
ddrew73 | 0:a7b02bcbe5fc | 76 | |
ddrew73 | 0:a7b02bcbe5fc | 77 | gx_d = (3.14/180) * (gx_d); |
ddrew73 | 0:a7b02bcbe5fc | 78 | gy_d = (3.14/180) * (gy_d); |
ddrew73 | 0:a7b02bcbe5fc | 79 | gz_d = (3.14/180) * (gz_d); |
ddrew73 | 0:a7b02bcbe5fc | 80 | |
ddrew73 | 0:a7b02bcbe5fc | 81 | //FOR DEBUGGING: |
ddrew73 | 0:a7b02bcbe5fc | 82 | //pc.printf("\n%1.3f %1.3f %1.3f \n", gx_d, gy_d, gz_d); |
ddrew73 | 0:a7b02bcbe5fc | 83 | |
ddrew73 | 0:a7b02bcbe5fc | 84 | q0dot = -q1*gx_d/2 - q2*gy_d/2 - q3*gz_d/2; |
ddrew73 | 0:a7b02bcbe5fc | 85 | q1dot = q0*gx_d/2 + q2*gz_d/2 - q3*gy_d/2; |
ddrew73 | 0:a7b02bcbe5fc | 86 | q2dot = q0*gy_d/2 - q1*gz_d/2 + q3*gx_d/2; |
ddrew73 | 0:a7b02bcbe5fc | 87 | q3dot = q0*gz_d/2 + q1*gy_d/2 - q2*gx_d/2; |
ddrew73 | 0:a7b02bcbe5fc | 88 | |
ddrew73 | 0:a7b02bcbe5fc | 89 | q0 += q0dot*ts; |
ddrew73 | 0:a7b02bcbe5fc | 90 | q1 += q1dot*ts; |
ddrew73 | 0:a7b02bcbe5fc | 91 | q2 += q2dot*ts; |
ddrew73 | 0:a7b02bcbe5fc | 92 | q3 += q3dot*ts; |
ddrew73 | 0:a7b02bcbe5fc | 93 | |
ddrew73 | 0:a7b02bcbe5fc | 94 | norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); |
ddrew73 | 0:a7b02bcbe5fc | 95 | |
ddrew73 | 0:a7b02bcbe5fc | 96 | q0 = q0/norm; |
ddrew73 | 0:a7b02bcbe5fc | 97 | q1 = q1/norm; |
ddrew73 | 0:a7b02bcbe5fc | 98 | q2 = q2/norm; |
ddrew73 | 0:a7b02bcbe5fc | 99 | q3 = q3/norm; |
ddrew73 | 0:a7b02bcbe5fc | 100 | |
ddrew73 | 0:a7b02bcbe5fc | 101 | theta = 180.0/3.14*atan2(2*(q0*q1 +q2*q3),1-2*(q1*q1+q2*q2)); |
ddrew73 | 0:a7b02bcbe5fc | 102 | phi = 180.0/3.14*asin(2*(q0*q2-q1*q3)); |
ddrew73 | 0:a7b02bcbe5fc | 103 | psi = 180.0/3.14*atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3)); |
ddrew73 | 0:a7b02bcbe5fc | 104 | |
ddrew73 | 0:a7b02bcbe5fc | 105 | pc.printf("%1.3f %1.3f %1.3f \n", theta, phi, psi); |
ddrew73 | 0:a7b02bcbe5fc | 106 | |
ddrew73 | 0:a7b02bcbe5fc | 107 | //Gestures defined: |
ddrew73 | 0:a7b02bcbe5fc | 108 | |
ddrew73 | 0:a7b02bcbe5fc | 109 | if (phi < -30){ |
ddrew73 | 0:a7b02bcbe5fc | 110 | float x, y, z; |
ddrew73 | 0:a7b02bcbe5fc | 111 | z = (acc.getAccZ()); |
ddrew73 | 0:a7b02bcbe5fc | 112 | if (overready == 1){ |
ddrew73 | 0:a7b02bcbe5fc | 113 | //key.printf("OVERHEAD! "); |
ddrew73 | 0:a7b02bcbe5fc | 114 | mouse.scroll(1); |
ddrew73 | 0:a7b02bcbe5fc | 115 | overready = 0; |
ddrew73 | 0:a7b02bcbe5fc | 116 | overheaded = 1; |
ddrew73 | 0:a7b02bcbe5fc | 117 | } |
ddrew73 | 0:a7b02bcbe5fc | 118 | if (z > 0.35 && totcount <5 && overheaded == 0){ |
ddrew73 | 0:a7b02bcbe5fc | 119 | upcount = upcount +1; |
ddrew73 | 0:a7b02bcbe5fc | 120 | totcount = totcount+1; |
ddrew73 | 0:a7b02bcbe5fc | 121 | } |
ddrew73 | 0:a7b02bcbe5fc | 122 | if (upcount > 2){ |
ddrew73 | 0:a7b02bcbe5fc | 123 | //key.printf("STAB! "); |
ddrew73 | 0:a7b02bcbe5fc | 124 | mouse.scroll(-1); |
ddrew73 | 0:a7b02bcbe5fc | 125 | totcount = 0; |
ddrew73 | 0:a7b02bcbe5fc | 126 | upcount = 0; |
ddrew73 | 0:a7b02bcbe5fc | 127 | } |
ddrew73 | 0:a7b02bcbe5fc | 128 | } |
ddrew73 | 0:a7b02bcbe5fc | 129 | |
ddrew73 | 0:a7b02bcbe5fc | 130 | if (phi > 30){ |
ddrew73 | 0:a7b02bcbe5fc | 131 | overready =1 ; |
ddrew73 | 0:a7b02bcbe5fc | 132 | } |
ddrew73 | 0:a7b02bcbe5fc | 133 | |
ddrew73 | 0:a7b02bcbe5fc | 134 | if (theta > 40 && blocked ==0){ |
ddrew73 | 0:a7b02bcbe5fc | 135 | //key.printf("BLOCK! "); |
ddrew73 | 0:a7b02bcbe5fc | 136 | mouse.press(MOUSE_RIGHT); |
ddrew73 | 0:a7b02bcbe5fc | 137 | mouse.release(MOUSE_RIGHT); |
ddrew73 | 0:a7b02bcbe5fc | 138 | blocked = 1; |
ddrew73 | 0:a7b02bcbe5fc | 139 | } |
ddrew73 | 0:a7b02bcbe5fc | 140 | |
ddrew73 | 0:a7b02bcbe5fc | 141 | if (theta < -40 && psi > 40 && struck == 0){ |
ddrew73 | 0:a7b02bcbe5fc | 142 | //key.printf("STRIKE! "); |
ddrew73 | 0:a7b02bcbe5fc | 143 | mouse.press(MOUSE_LEFT); |
ddrew73 | 0:a7b02bcbe5fc | 144 | mouse.release(MOUSE_LEFT); |
ddrew73 | 0:a7b02bcbe5fc | 145 | struck = 1; |
ddrew73 | 0:a7b02bcbe5fc | 146 | } |
ddrew73 | 0:a7b02bcbe5fc | 147 | if (theta < -40 && psi < 30){ |
ddrew73 | 0:a7b02bcbe5fc | 148 | struck = 0; |
ddrew73 | 0:a7b02bcbe5fc | 149 | } |
ddrew73 | 0:a7b02bcbe5fc | 150 | |
ddrew73 | 0:a7b02bcbe5fc | 151 | //Rest reset for gyros: |
ddrew73 | 0:a7b02bcbe5fc | 152 | float x, y, z; |
ddrew73 | 0:a7b02bcbe5fc | 153 | x = abs(acc.getAccX()); |
ddrew73 | 0:a7b02bcbe5fc | 154 | y = abs(acc.getAccY()); |
ddrew73 | 0:a7b02bcbe5fc | 155 | z = abs(acc.getAccZ()); |
ddrew73 | 0:a7b02bcbe5fc | 156 | pc.printf("%1.3f %1.3f %1.3f \n", x, y, z); |
ddrew73 | 0:a7b02bcbe5fc | 157 | if (z > 0.9 && x < 0.3 && y < 0.3){ |
ddrew73 | 0:a7b02bcbe5fc | 158 | restcount = restcount + 1; |
ddrew73 | 0:a7b02bcbe5fc | 159 | } |
ddrew73 | 0:a7b02bcbe5fc | 160 | else |
ddrew73 | 0:a7b02bcbe5fc | 161 | restcount = 0; |
ddrew73 | 0:a7b02bcbe5fc | 162 | |
ddrew73 | 0:a7b02bcbe5fc | 163 | if (restcount > 1){ |
ddrew73 | 0:a7b02bcbe5fc | 164 | q0 = 1, q1 = 0, q2 = 0, q3 = 0; |
ddrew73 | 0:a7b02bcbe5fc | 165 | q0dot = 0, q1dot = 0, q2dot = 0, q3dot = 0; |
ddrew73 | 0:a7b02bcbe5fc | 166 | theta = 0; |
ddrew73 | 0:a7b02bcbe5fc | 167 | phi = 0; |
ddrew73 | 0:a7b02bcbe5fc | 168 | psi = 0; |
ddrew73 | 0:a7b02bcbe5fc | 169 | blocked = 0; |
ddrew73 | 0:a7b02bcbe5fc | 170 | overheaded = 0; |
ddrew73 | 0:a7b02bcbe5fc | 171 | } |
ddrew73 | 0:a7b02bcbe5fc | 172 | |
ddrew73 | 0:a7b02bcbe5fc | 173 | //Update interval; must be synced with gyro timestep! |
ddrew73 | 0:a7b02bcbe5fc | 174 | wait(0.1); |
ddrew73 | 0:a7b02bcbe5fc | 175 | |
ddrew73 | 0:a7b02bcbe5fc | 176 | |
ddrew73 | 0:a7b02bcbe5fc | 177 | } |
ddrew73 | 0:a7b02bcbe5fc | 178 | } |