File content as of revision 1:bb8f49838af8:

/*
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.3 && 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 > 25){
        overready =1 ;
        }
    
    if (theta > 55 && blocked ==0 && phi > - 20){
        //key.printf("BLOCK!  ");
        mouse.press(MOUSE_RIGHT);
        mouse.release(MOUSE_RIGHT);
        blocked = 1;
        }

    if (theta < -25 && psi > 30 && struck == 0){
        //key.printf("STRIKE!  ");
        mouse.press(MOUSE_LEFT);
        mouse.release(MOUSE_LEFT);
        struck = 1;
        }
    if (theta < -30 && psi < 20){
        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);
  

}
}