main.cpp

00001 #include "mbed.h"
00002 #include "Quaternion.h"
00003 #include "LSM9DS0.h"
00004 #include "USBKeyboard.h"
00005 
00006 Serial pc(USBTX, USBRX);
00007 //IMU
00008 // SDO_XM and SDO_G are pulled up, so our addresses are:
00009 #define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
00010 #define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
00011 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
00012 Quaternion q;
00013 #define M_PI 3.14159265
00014 
00015 float accLin[3]; // linear accelerations 
00016 double ypr[3]; //yaw pitch roll
00017 
00018 USBKeyboard keyboard;
00019 USBKeyboard keyboard2;
00020 
00021 DigitalIn left(p15);
00022 DigitalIn right(p16);
00023 
00024 int main() {
00025     
00026     //IMU
00027     uint16_t status = imu.begin();
00028     double test;
00029     //Make sure communication is working
00030     pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
00031     pc.printf("Should be 0x49D4\n\n");
00032     float xjump = 9; // threshold for jump
00033     float yjump = 1.15; // g threshold
00034     float zright = .4; // move right z acc threshold
00035     float zleft = -.2; // move left z acc threshold
00036     float rollRight = -120; // be less than this to move right
00037     float rollLeft = -80; // be greater than this to move left
00038 
00039     q = Quaternion();
00040     while(1) {
00041     imu.readAccel();
00042     imu.readGyro();
00043     imu.readMag();
00044     //t2.start();
00045     //pc.printf("ax= %f, ay = %f, az = %f \n", imu.ax,imu.ay,imu.az);
00046     //pc.printf("gx= %f, gy = %f, gz = %f \n", imu.gx,imu.gy,imu.gz);
00047     
00048     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);
00049     q.getLinearAcceleration(accLin, imu.ax,imu.ay,imu.az);
00050     q.getYawPitchRoll(ypr);
00051     
00052     //pc.printf("xl= %f, yl= %f, zl= %f \n \n",accLin[0],accLin[1],accLin[2]);
00053     //pc.printf("y= %f, p= %f, r= %f \n \n",ypr[0],ypr[1],ypr[2]);
00054     /*
00055     pc.printf("p = %f  ", ypr[1]);
00056     pc.printf("r = %f   ",ypr[2]);
00057     pc.printf("a = %f   ",accLin[1]);
00058     pc. printf("ax = %f   ", accLin[0]);
00059     pc.printf("imuax = %f   ", imu.ax);
00060     pc.printf("imuay = %f   ", imu.ay);  
00061     pc.printf("imuaz = %f \n", imu.az);
00062     */
00063     pc.printf("%f,%f \n",ypr[2],imu.az);
00064     
00065     
00066     /*
00067     
00068     if (ypr[2] > rollLeft | left)
00069     {
00070         keyboard.keyCode(LEFT_ARROW);
00071 
00072         }
00073     else if (ypr[2] < rollRight)
00074     {
00075         keyboard.keyCode(RIGHT_ARROW);
00076         }
00077     else
00078     {
00079         keyboard.keyCodeOff(LEFT_ARROW);
00080         }
00081         */
00082         
00083     if (imu.az > zright)
00084     {keyboard.keyCode(RIGHT_ARROW);}
00085     else if (imu.az < zleft)
00086     {keyboard.keyCode(LEFT_ARROW);}
00087     else
00088     {keyboard.keyCodeOff(LEFT_ARROW);}
00089     if(imu.ay > yjump)
00090     {
00091         for(int i =0; i < 100; i++)
00092         {keyboard.keyCode(UP_ARROW);}
00093         keyboard.keyCodeOff(UP_ARROW);
00094         }
00095 
00096     
00097     /*
00098     if (ypr[2] < rollLeft | left)
00099     {
00100         keyboard.keyCodeOff(LEFT_ARROW);
00101         }
00102 
00103     if (ypr[2] < rollRight | right)
00104     {
00105         keyboard2.keyCode(RIGHT_ARROW);
00106         }
00107     if (ypr[2] > rollRight | right)
00108     {
00109         keyboard2.keyCodeOff(RIGHT_ARROW);
00110       
00111         } */
00112 /*
00113     if (accLin[1] > yjump)
00114     {
00115         keyboard.keyCode(UP_ARROW);
00116         pc.printf("here3");
00117         }
00118   */
00119     //keyboard.keyCode(RIGHT_ARROW);
00120     
00121     //wait(1.0);
00122 
00123     }
00124 }
00125