4180 final project
Dependencies: LSM9DS0 USBDevice mbed
Diff: main.cpp
- Revision:
- 0:ebbc3cd3a61e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Dec 05 18:39:33 2015 +0000 @@ -0,0 +1,125 @@ +#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); + + } +} +