jaewon lee / Mbed 2 deprecated 4180finalproject

4180 final project

Dependencies:   LSM9DS0 USBDevice mbed

main.cpp@0:ebbc3cd3a61e, 2015-12-05 (annotated)

Committer:
jlee887
Date:
Sat Dec 05 18:39:33 2015 +0000
Revision:
0:ebbc3cd3a61e
d

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jlee887 0:ebbc3cd3a61e 1 #include "mbed.h"
jlee887 0:ebbc3cd3a61e 2 #include "Quaternion.h"
jlee887 0:ebbc3cd3a61e 3 #include "LSM9DS0.h"
jlee887 0:ebbc3cd3a61e 4 #include "USBKeyboard.h"
jlee887 0:ebbc3cd3a61e 5
jlee887 0:ebbc3cd3a61e 6 Serial pc(USBTX, USBRX);
jlee887 0:ebbc3cd3a61e 7 //IMU
jlee887 0:ebbc3cd3a61e 8 // SDO_XM and SDO_G are pulled up, so our addresses are:
jlee887 0:ebbc3cd3a61e 9 #define LSM9DS0_XM_ADDR 0x1D // Would be 0x1E if SDO_XM is LOW
jlee887 0:ebbc3cd3a61e 10 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
jlee887 0:ebbc3cd3a61e 11 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
jlee887 0:ebbc3cd3a61e 12 Quaternion q;
jlee887 0:ebbc3cd3a61e 13 #define M_PI 3.14159265
jlee887 0:ebbc3cd3a61e 14
jlee887 0:ebbc3cd3a61e 15 float accLin[3]; // linear accelerations
jlee887 0:ebbc3cd3a61e 16 double ypr[3]; //yaw pitch roll
jlee887 0:ebbc3cd3a61e 17
jlee887 0:ebbc3cd3a61e 18 USBKeyboard keyboard;
jlee887 0:ebbc3cd3a61e 19 USBKeyboard keyboard2;
jlee887 0:ebbc3cd3a61e 20
jlee887 0:ebbc3cd3a61e 21 DigitalIn left(p15);
jlee887 0:ebbc3cd3a61e 22 DigitalIn right(p16);
jlee887 0:ebbc3cd3a61e 23
jlee887 0:ebbc3cd3a61e 24 int main() {
jlee887 0:ebbc3cd3a61e 25
jlee887 0:ebbc3cd3a61e 26 //IMU
jlee887 0:ebbc3cd3a61e 27 uint16_t status = imu.begin();
jlee887 0:ebbc3cd3a61e 28 double test;
jlee887 0:ebbc3cd3a61e 29 //Make sure communication is working
jlee887 0:ebbc3cd3a61e 30 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
jlee887 0:ebbc3cd3a61e 31 pc.printf("Should be 0x49D4\n\n");
jlee887 0:ebbc3cd3a61e 32 float xjump = 9; // threshold for jump
jlee887 0:ebbc3cd3a61e 33 float yjump = 1.15; // g threshold
jlee887 0:ebbc3cd3a61e 34 float zright = .4; // move right z acc threshold
jlee887 0:ebbc3cd3a61e 35 float zleft = -.2; // move left z acc threshold
jlee887 0:ebbc3cd3a61e 36 float rollRight = -120; // be less than this to move right
jlee887 0:ebbc3cd3a61e 37 float rollLeft = -80; // be greater than this to move left
jlee887 0:ebbc3cd3a61e 38
jlee887 0:ebbc3cd3a61e 39 q = Quaternion();
jlee887 0:ebbc3cd3a61e 40 while(1) {
jlee887 0:ebbc3cd3a61e 41 imu.readAccel();
jlee887 0:ebbc3cd3a61e 42 imu.readGyro();
jlee887 0:ebbc3cd3a61e 43 imu.readMag();
jlee887 0:ebbc3cd3a61e 44 //t2.start();
jlee887 0:ebbc3cd3a61e 45 //pc.printf("ax= %f, ay = %f, az = %f \n", imu.ax,imu.ay,imu.az);
jlee887 0:ebbc3cd3a61e 46 //pc.printf("gx= %f, gy = %f, gz = %f \n", imu.gx,imu.gy,imu.gz);
jlee887 0:ebbc3cd3a61e 47
jlee887 0:ebbc3cd3a61e 48 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);
jlee887 0:ebbc3cd3a61e 49 q.getLinearAcceleration(accLin, imu.ax,imu.ay,imu.az);
jlee887 0:ebbc3cd3a61e 50 q.getYawPitchRoll(ypr);
jlee887 0:ebbc3cd3a61e 51
jlee887 0:ebbc3cd3a61e 52 //pc.printf("xl= %f, yl= %f, zl= %f \n \n",accLin[0],accLin[1],accLin[2]);
jlee887 0:ebbc3cd3a61e 53 //pc.printf("y= %f, p= %f, r= %f \n \n",ypr[0],ypr[1],ypr[2]);
jlee887 0:ebbc3cd3a61e 54 /*
jlee887 0:ebbc3cd3a61e 55 pc.printf("p = %f ", ypr[1]);
jlee887 0:ebbc3cd3a61e 56 pc.printf("r = %f ",ypr[2]);
jlee887 0:ebbc3cd3a61e 57 pc.printf("a = %f ",accLin[1]);
jlee887 0:ebbc3cd3a61e 58 pc. printf("ax = %f ", accLin[0]);
jlee887 0:ebbc3cd3a61e 59 pc.printf("imuax = %f ", imu.ax);
jlee887 0:ebbc3cd3a61e 60 pc.printf("imuay = %f ", imu.ay);
jlee887 0:ebbc3cd3a61e 61 pc.printf("imuaz = %f \n", imu.az);
jlee887 0:ebbc3cd3a61e 62 */
jlee887 0:ebbc3cd3a61e 63 pc.printf("%f,%f \n",ypr[2],imu.az);
jlee887 0:ebbc3cd3a61e 64
jlee887 0:ebbc3cd3a61e 65
jlee887 0:ebbc3cd3a61e 66 /*
jlee887 0:ebbc3cd3a61e 67
jlee887 0:ebbc3cd3a61e 68 if (ypr[2] > rollLeft | left)
jlee887 0:ebbc3cd3a61e 69 {
jlee887 0:ebbc3cd3a61e 70 keyboard.keyCode(LEFT_ARROW);
jlee887 0:ebbc3cd3a61e 71
jlee887 0:ebbc3cd3a61e 72 }
jlee887 0:ebbc3cd3a61e 73 else if (ypr[2] < rollRight)
jlee887 0:ebbc3cd3a61e 74 {
jlee887 0:ebbc3cd3a61e 75 keyboard.keyCode(RIGHT_ARROW);
jlee887 0:ebbc3cd3a61e 76 }
jlee887 0:ebbc3cd3a61e 77 else
jlee887 0:ebbc3cd3a61e 78 {
jlee887 0:ebbc3cd3a61e 79 keyboard.keyCodeOff(LEFT_ARROW);
jlee887 0:ebbc3cd3a61e 80 }
jlee887 0:ebbc3cd3a61e 81 */
jlee887 0:ebbc3cd3a61e 82
jlee887 0:ebbc3cd3a61e 83 if (imu.az > zright)
jlee887 0:ebbc3cd3a61e 84 {keyboard.keyCode(RIGHT_ARROW);}
jlee887 0:ebbc3cd3a61e 85 else if (imu.az < zleft)
jlee887 0:ebbc3cd3a61e 86 {keyboard.keyCode(LEFT_ARROW);}
jlee887 0:ebbc3cd3a61e 87 else
jlee887 0:ebbc3cd3a61e 88 {keyboard.keyCodeOff(LEFT_ARROW);}
jlee887 0:ebbc3cd3a61e 89 if(imu.ay > yjump)
jlee887 0:ebbc3cd3a61e 90 {
jlee887 0:ebbc3cd3a61e 91 for(int i =0; i < 100; i++)
jlee887 0:ebbc3cd3a61e 92 {keyboard.keyCode(UP_ARROW);}
jlee887 0:ebbc3cd3a61e 93 keyboard.keyCodeOff(UP_ARROW);
jlee887 0:ebbc3cd3a61e 94 }
jlee887 0:ebbc3cd3a61e 95
jlee887 0:ebbc3cd3a61e 96
jlee887 0:ebbc3cd3a61e 97 /*
jlee887 0:ebbc3cd3a61e 98 if (ypr[2] < rollLeft | left)
jlee887 0:ebbc3cd3a61e 99 {
jlee887 0:ebbc3cd3a61e 100 keyboard.keyCodeOff(LEFT_ARROW);
jlee887 0:ebbc3cd3a61e 101 }
jlee887 0:ebbc3cd3a61e 102
jlee887 0:ebbc3cd3a61e 103 if (ypr[2] < rollRight | right)
jlee887 0:ebbc3cd3a61e 104 {
jlee887 0:ebbc3cd3a61e 105 keyboard2.keyCode(RIGHT_ARROW);
jlee887 0:ebbc3cd3a61e 106 }
jlee887 0:ebbc3cd3a61e 107 if (ypr[2] > rollRight | right)
jlee887 0:ebbc3cd3a61e 108 {
jlee887 0:ebbc3cd3a61e 109 keyboard2.keyCodeOff(RIGHT_ARROW);
jlee887 0:ebbc3cd3a61e 110
jlee887 0:ebbc3cd3a61e 111 } */
jlee887 0:ebbc3cd3a61e 112 /*
jlee887 0:ebbc3cd3a61e 113 if (accLin[1] > yjump)
jlee887 0:ebbc3cd3a61e 114 {
jlee887 0:ebbc3cd3a61e 115 keyboard.keyCode(UP_ARROW);
jlee887 0:ebbc3cd3a61e 116 pc.printf("here3");
jlee887 0:ebbc3cd3a61e 117 }
jlee887 0:ebbc3cd3a61e 118 */
jlee887 0:ebbc3cd3a61e 119 //keyboard.keyCode(RIGHT_ARROW);
jlee887 0:ebbc3cd3a61e 120
jlee887 0:ebbc3cd3a61e 121 //wait(1.0);
jlee887 0:ebbc3cd3a61e 122
jlee887 0:ebbc3cd3a61e 123 }
jlee887 0:ebbc3cd3a61e 124 }
jlee887 0:ebbc3cd3a61e 125