Interactive Device Design
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
Revision 0:a7b02bcbe5fc, committed 2014-09-29
- Comitter:
- ddrew73
- Date:
- Mon Sep 29 01:52:45 2014 +0000
- Commit message:
- Nothin.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DebounceIn.lib Mon Sep 29 01:52:45 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/DebounceIn/#31ae5cfb44a4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L3GD20.lib Mon Sep 29 01:52:45 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/bclaus/code/L3GD20/#62dfce144cf7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MMA8451Q.lib Mon Sep 29 01:52:45 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/emilmont/code/MMA8451Q/#c4d879a39775
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice.lib Mon Sep 29 01:52:45 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/USBDevice/#5bf05f9b3c7b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Sep 29 01:52:45 2014 +0000
@@ -0,0 +1,178 @@
+/*
+CS294 Interactive Device Design - Homework 3
+CLASH! Sword
+
+Daniel Drew
+Filip Maksimovic
+*/
+
+#include "mbed.h"
+#include "MMA8451Q.h"
+#include "DebounceIn.h"
+#include "L3GD20.h"
+#include "USBMouse.h"
+
+//FOR GESTURE DEBUGGING! Make sure to uncomment the object below!
+//#include "USBKeyboard.h"
+
+//Define I2C stuff for Accel, Gyro
+PinName const SDA1 = PTE25;
+PinName const SCL1 = PTE24;
+PinName const SDA2 = PTE0;
+PinName const SCL2 = PTE1;
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+int main() {
+
+//Object Instantiation
+Serial pc(USBTX, USBRX); // tx, rx
+USBMouse mouse;
+MMA8451Q acc(SDA1, SCL1, MMA8451_I2C_ADDRESS);
+DigitalOut myred(LED1);
+DigitalOut mygreen(LED2);
+DigitalOut myblue(LED3);
+L3GD20 gyro(SDA2, SCL2);
+//USBKeyboard key;
+
+//Variable Instantiation
+int i=0;
+int upcount = 0, totcount = 0, overready = 0, blocked = 0, struck = 0, restcount = 0, overheaded = 0;
+float gx_d, gy_d, gz_d;
+float gx, gy, gz;
+float gx_zero = 0.0, gy_zero = 0.0, gz_zero = 0.0;
+double ts = 0.1 , norm = 0;
+double q0 = 1, q1 = 0, q2 = 0, q3 = 0;
+double q0dot = 0, q1dot = 0, q2dot = 0, q3dot = 0;
+double theta = 0, phi = 0, psi = 0;
+
+//Begin main program loop
+while(1){
+
+ //Start-up gyro "calibration" - LED turns red when it is complete
+ myred = 1;
+ mygreen = 1;
+ myblue = 0;
+ while(i < 10000){
+ gyro.read(&gx, &gy, &gz);
+ gx_zero = gx_zero + gx;
+ gy_zero = gy_zero + gy;
+ gz_zero = gz_zero + gz;
+ i = i+1;
+ }
+ myred = 0;
+ mygreen = 1;
+ myblue = 1;
+
+ //Gyro quaternion stuff
+ gyro.read(&gx, &gy, &gz);
+ gx_d = gx - (gx_zero / 10000 );
+ gy_d = gy - (gy_zero / 10000 );
+ gz_d = gz - (gz_zero / 10000 );
+
+ gx_d = (double)gx_d;
+ gy_d = (double)gy_d;
+ gz_d = (double)gz_d;
+
+ gx_d = (3.14/180) * (gx_d);
+ gy_d = (3.14/180) * (gy_d);
+ gz_d = (3.14/180) * (gz_d);
+
+ //FOR DEBUGGING:
+ //pc.printf("\n%1.3f %1.3f %1.3f \n", gx_d, gy_d, gz_d);
+
+ q0dot = -q1*gx_d/2 - q2*gy_d/2 - q3*gz_d/2;
+ q1dot = q0*gx_d/2 + q2*gz_d/2 - q3*gy_d/2;
+ q2dot = q0*gy_d/2 - q1*gz_d/2 + q3*gx_d/2;
+ q3dot = q0*gz_d/2 + q1*gy_d/2 - q2*gx_d/2;
+
+ q0 += q0dot*ts;
+ q1 += q1dot*ts;
+ q2 += q2dot*ts;
+ q3 += q3dot*ts;
+
+ norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+
+ q0 = q0/norm;
+ q1 = q1/norm;
+ q2 = q2/norm;
+ q3 = q3/norm;
+
+ theta = 180.0/3.14*atan2(2*(q0*q1 +q2*q3),1-2*(q1*q1+q2*q2));
+ phi = 180.0/3.14*asin(2*(q0*q2-q1*q3));
+ psi = 180.0/3.14*atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
+
+ pc.printf("%1.3f %1.3f %1.3f \n", theta, phi, psi);
+
+ //Gestures defined:
+
+ if (phi < -30){
+ float x, y, z;
+ z = (acc.getAccZ());
+ if (overready == 1){
+ //key.printf("OVERHEAD! ");
+ mouse.scroll(1);
+ overready = 0;
+ overheaded = 1;
+ }
+ if (z > 0.35 && totcount <5 && overheaded == 0){
+ upcount = upcount +1;
+ totcount = totcount+1;
+ }
+ if (upcount > 2){
+ //key.printf("STAB! ");
+ mouse.scroll(-1);
+ totcount = 0;
+ upcount = 0;
+ }
+ }
+
+ if (phi > 30){
+ overready =1 ;
+ }
+
+ if (theta > 40 && blocked ==0){
+ //key.printf("BLOCK! ");
+ mouse.press(MOUSE_RIGHT);
+ mouse.release(MOUSE_RIGHT);
+ blocked = 1;
+ }
+
+ if (theta < -40 && psi > 40 && struck == 0){
+ //key.printf("STRIKE! ");
+ mouse.press(MOUSE_LEFT);
+ mouse.release(MOUSE_LEFT);
+ struck = 1;
+ }
+ if (theta < -40 && psi < 30){
+ struck = 0;
+ }
+
+ //Rest reset for gyros:
+ float x, y, z;
+ x = abs(acc.getAccX());
+ y = abs(acc.getAccY());
+ z = abs(acc.getAccZ());
+ pc.printf("%1.3f %1.3f %1.3f \n", x, y, z);
+ if (z > 0.9 && x < 0.3 && y < 0.3){
+ restcount = restcount + 1;
+ }
+ else
+ restcount = 0;
+
+ if (restcount > 1){
+ q0 = 1, q1 = 0, q2 = 0, q3 = 0;
+ q0dot = 0, q1dot = 0, q2dot = 0, q3dot = 0;
+ theta = 0;
+ phi = 0;
+ psi = 0;
+ blocked = 0;
+ overheaded = 0;
+ }
+
+ //Update interval; must be synced with gyro timestep!
+ wait(0.1);
+
+
+}
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Sep 29 01:52:45 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/04dd9b1680ae \ No newline at end of file