Daniel Drew / Mbed 2 deprecated idd_hw3_ddrew73_clash

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

Fork of idd_hw3_ddrew73_fil_clashSword by Interactive Device Design

main.cpp@1:bb8f49838af8, 2017-06-12 (annotated)

Committer:
ddrew73
Date:
Mon Jun 12 20:02:23 2017 +0000
Revision:
1:bb8f49838af8
Parent:
0:a7b02bcbe5fc 
Testing self account publish;

Who changed what in which revision?

UserRevisionLine numberNew 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 1:bb8f49838af8 118 if (z > 0.3 && 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 1:bb8f49838af8 130 if (phi > 25){
ddrew73 0:a7b02bcbe5fc 131 overready =1 ;
ddrew73 0:a7b02bcbe5fc 132 }
ddrew73 0:a7b02bcbe5fc 133
ddrew73 1:bb8f49838af8 134 if (theta > 55 && blocked ==0 && phi > - 20){
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 1:bb8f49838af8 141 if (theta < -25 && psi > 30 && 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 1:bb8f49838af8 147 if (theta < -30 && psi < 20){
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 }