4180 final project

Dependencies:   LSM9DS0 USBDevice mbed

main.cpp

Committer:
jlee887
Date:
2015-12-05
Revision:
0:ebbc3cd3a61e

File content as of revision 0:ebbc3cd3a61e:

#include "mbed.h"
#include "Quaternion.h"
#include "LSM9DS0.h"
#include "USBKeyboard.h"

Serial pc(USBTX, USBRX);
//IMU
// SDO_XM and SDO_G are pulled up, so our addresses are:
#define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
Quaternion q;
#define M_PI 3.14159265

float accLin[3]; // linear accelerations 
double ypr[3]; //yaw pitch roll

USBKeyboard keyboard;
USBKeyboard keyboard2;

DigitalIn left(p15);
DigitalIn right(p16);

int main() {
    
    //IMU
    uint16_t status = imu.begin();
    double test;
    //Make sure communication is working
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
    pc.printf("Should be 0x49D4\n\n");
    float xjump = 9; // threshold for jump
    float yjump = 1.15; // g threshold
    float zright = .4; // move right z acc threshold
    float zleft = -.2; // move left z acc threshold
    float rollRight = -120; // be less than this to move right
    float rollLeft = -80; // be greater than this to move left

    q = Quaternion();
    while(1) {
    imu.readAccel();
    imu.readGyro();
    imu.readMag();
    //t2.start();
    //pc.printf("ax= %f, ay = %f, az = %f \n", imu.ax,imu.ay,imu.az);
    //pc.printf("gx= %f, gy = %f, gz = %f \n", imu.gx,imu.gy,imu.gz);
    
    q.update9DOF(imu.gx*M_PI/180, imu.gy*M_PI/180, imu.gz*M_PI/180, imu.ax, imu.ay, imu.az, imu.mx, imu.my, imu.mz);
    q.getLinearAcceleration(accLin, imu.ax,imu.ay,imu.az);
    q.getYawPitchRoll(ypr);
    
    //pc.printf("xl= %f, yl= %f, zl= %f \n \n",accLin[0],accLin[1],accLin[2]);
    //pc.printf("y= %f, p= %f, r= %f \n \n",ypr[0],ypr[1],ypr[2]);
    /*
    pc.printf("p = %f  ", ypr[1]);
    pc.printf("r = %f   ",ypr[2]);
    pc.printf("a = %f   ",accLin[1]);
    pc. printf("ax = %f   ", accLin[0]);
    pc.printf("imuax = %f   ", imu.ax);
    pc.printf("imuay = %f   ", imu.ay);  
    pc.printf("imuaz = %f \n", imu.az);
    */
    pc.printf("%f,%f \n",ypr[2],imu.az);
    
    
    /*
    
    if (ypr[2] > rollLeft | left)
    {
        keyboard.keyCode(LEFT_ARROW);

        }
    else if (ypr[2] < rollRight)
    {
        keyboard.keyCode(RIGHT_ARROW);
        }
    else
    {
        keyboard.keyCodeOff(LEFT_ARROW);
        }
        */
        
    if (imu.az > zright)
    {keyboard.keyCode(RIGHT_ARROW);}
    else if (imu.az < zleft)
    {keyboard.keyCode(LEFT_ARROW);}
    else
    {keyboard.keyCodeOff(LEFT_ARROW);}
    if(imu.ay > yjump)
    {
        for(int i =0; i < 100; i++)
        {keyboard.keyCode(UP_ARROW);}
        keyboard.keyCodeOff(UP_ARROW);
        }

    
    /*
    if (ypr[2] < rollLeft | left)
    {
        keyboard.keyCodeOff(LEFT_ARROW);
        }

    if (ypr[2] < rollRight | right)
    {
        keyboard2.keyCode(RIGHT_ARROW);
        }
    if (ypr[2] > rollRight | right)
    {
        keyboard2.keyCodeOff(RIGHT_ARROW);
      
        } */
/*
    if (accLin[1] > yjump)
    {
        keyboard.keyCode(UP_ARROW);
        pc.printf("here3");
        }
  */
    //keyboard.keyCode(RIGHT_ARROW);
    
    //wait(1.0);

    }
}