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); } }