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