main.cpp

00001 /*
00002 CS294 Interactive Device Design - Homework 3
00003 CLASH! Sword
00004 
00005 Daniel Drew
00006 Filip Maksimovic
00007 */
00008 
00009 #include "mbed.h"
00010 #include "MMA8451Q.h"
00011 #include "DebounceIn.h"
00012 #include "L3GD20.h"
00013 #include "USBMouse.h"
00014 
00015 //FOR GESTURE DEBUGGING! Make sure to uncomment the object below!
00016 //#include "USBKeyboard.h"
00017 
00018 //Define I2C stuff for Accel, Gyro
00019 PinName const SDA1 = PTE25;
00020 PinName const SCL1 = PTE24;
00021 PinName const SDA2 = PTE0;
00022 PinName const SCL2 = PTE1;
00023 
00024 #define MMA8451_I2C_ADDRESS (0x1d<<1)
00025 
00026 int main() {
00027     
00028 //Object Instantiation    
00029 Serial pc(USBTX, USBRX); // tx, rx
00030 USBMouse mouse;
00031 MMA8451Q acc(SDA1, SCL1, MMA8451_I2C_ADDRESS);
00032 DigitalOut myred(LED1);
00033 DigitalOut mygreen(LED2);
00034 DigitalOut myblue(LED3);
00035 L3GD20 gyro(SDA2, SCL2);
00036 //USBKeyboard key;
00037 
00038 //Variable Instantiation
00039 int i=0;
00040 int upcount = 0, totcount = 0, overready = 0, blocked = 0, struck = 0, restcount = 0, overheaded = 0;
00041 float gx_d, gy_d, gz_d; 
00042 float gx, gy, gz;
00043 float gx_zero = 0.0, gy_zero = 0.0, gz_zero = 0.0;
00044 double ts = 0.1 , norm = 0;
00045 double q0 = 1, q1 = 0, q2 = 0, q3 = 0; 
00046 double q0dot = 0, q1dot = 0, q2dot = 0, q3dot = 0;
00047 double theta = 0, phi = 0, psi = 0;
00048 
00049 //Begin main program loop
00050 while(1){
00051 
00052     //Start-up gyro "calibration" - LED turns red when it is complete
00053     myred = 1;
00054     mygreen = 1;
00055     myblue = 0;
00056     while(i < 10000){
00057         gyro.read(&gx, &gy, &gz);
00058         gx_zero = gx_zero + gx;
00059         gy_zero = gy_zero + gy;
00060         gz_zero = gz_zero + gz;
00061         i = i+1;
00062         }
00063     myred = 0;
00064     mygreen = 1;
00065     myblue = 1;
00066 
00067     //Gyro quaternion stuff
00068     gyro.read(&gx, &gy, &gz);
00069     gx_d = gx - (gx_zero / 10000 );
00070     gy_d = gy - (gy_zero / 10000 );
00071     gz_d = gz - (gz_zero / 10000 );
00072    
00073     gx_d = (double)gx_d;
00074     gy_d = (double)gy_d;
00075     gz_d = (double)gz_d;
00076     
00077     gx_d =  (3.14/180) * (gx_d);
00078     gy_d =  (3.14/180) * (gy_d);
00079     gz_d =  (3.14/180) * (gz_d);
00080     
00081     //FOR DEBUGGING:
00082     //pc.printf("\n%1.3f %1.3f %1.3f \n", gx_d, gy_d, gz_d); 
00083   
00084     q0dot = -q1*gx_d/2 - q2*gy_d/2 - q3*gz_d/2;
00085     q1dot = q0*gx_d/2 + q2*gz_d/2 - q3*gy_d/2;
00086     q2dot = q0*gy_d/2 - q1*gz_d/2 + q3*gx_d/2;
00087     q3dot = q0*gz_d/2 + q1*gy_d/2 - q2*gx_d/2;
00088     
00089     q0 += q0dot*ts; 
00090     q1 += q1dot*ts; 
00091     q2 += q2dot*ts; 
00092     q3 += q3dot*ts; 
00093     
00094     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00095     
00096     q0 = q0/norm;
00097     q1 = q1/norm;
00098     q2 = q2/norm;
00099     q3 = q3/norm;
00100     
00101     theta = 180.0/3.14*atan2(2*(q0*q1 +q2*q3),1-2*(q1*q1+q2*q2));
00102     phi = 180.0/3.14*asin(2*(q0*q2-q1*q3));
00103     psi = 180.0/3.14*atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
00104     
00105     pc.printf("%1.3f %1.3f %1.3f \n", theta, phi, psi); 
00106     
00107     //Gestures defined:
00108     
00109     if (phi < -30){
00110         float x, y, z;
00111         z = (acc.getAccZ()); 
00112         if (overready == 1){
00113             //key.printf("OVERHEAD! ");
00114             mouse.scroll(1);
00115             overready = 0;
00116             overheaded = 1;
00117             }
00118         if (z > 0.35 && totcount <5 && overheaded == 0){
00119             upcount = upcount +1;
00120             totcount = totcount+1;
00121             }
00122         if (upcount > 2){
00123             //key.printf("STAB! ");
00124             mouse.scroll(-1);
00125             totcount = 0;
00126             upcount = 0;
00127             }
00128         }
00129     
00130     if (phi > 30){
00131         overready =1 ;
00132         }
00133     
00134     if (theta > 40 && blocked ==0){
00135         //key.printf("BLOCK!  ");
00136         mouse.press(MOUSE_RIGHT);
00137         mouse.release(MOUSE_RIGHT);
00138         blocked = 1;
00139         }
00140 
00141     if (theta < -40 && psi > 40 && struck == 0){
00142         //key.printf("STRIKE!  ");
00143         mouse.press(MOUSE_LEFT);
00144         mouse.release(MOUSE_LEFT);
00145         struck = 1;
00146         }
00147     if (theta < -40 && psi < 30){
00148         struck = 0;
00149         }
00150     
00151     //Rest reset for gyros:
00152     float x, y, z;
00153         x = abs(acc.getAccX()); 
00154         y = abs(acc.getAccY()); 
00155         z = abs(acc.getAccZ()); 
00156         pc.printf("%1.3f %1.3f %1.3f \n", x, y, z); 
00157         if (z > 0.9 && x < 0.3 && y < 0.3){
00158             restcount = restcount + 1;
00159             }
00160         else 
00161             restcount = 0;
00162             
00163         if (restcount > 1){
00164             q0 = 1, q1 = 0, q2 = 0, q3 = 0; 
00165             q0dot = 0, q1dot = 0, q2dot = 0, q3dot = 0;
00166             theta = 0;
00167             phi = 0; 
00168             psi = 0;
00169             blocked = 0;
00170             overheaded = 0;
00171             }
00172     
00173     //Update interval; must be synced with gyro timestep!         
00174     wait(0.1);
00175   
00176 
00177 }
00178 }