Terrabots / Mbed 2 deprecated IMU

General IMU Library

Dependencies:   mbed LSM9DS1_Library

IMU.cpp@4:ab08a70e0d7d, 2017-02-16 (annotated)

Committer:
simplyellow
Date:
Thu Feb 16 20:31:35 2017 +0000
Revision:
4:ab08a70e0d7d
Parent:
2:ae1bfb236387 
fixed compile issues.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jcallahan1 2:ae1bfb236387 1 #include "IMU.h"
jcallahan1 2:ae1bfb236387 2
simplyellow 4:ab08a70e0d7d 3 #define PI 3.14159
jcallahan1 2:ae1bfb236387 4
jcallahan1 2:ae1bfb236387 5 IMU::IMU() {
simplyellow 4:ab08a70e0d7d 6 I = new LSM9DS1(p9, p10, 0xD6, 0x3C);
simplyellow 4:ab08a70e0d7d 7 I->begin();
simplyellow 4:ab08a70e0d7d 8 if (!I->begin()) {
jcallahan1 2:ae1bfb236387 9 printf("Failed to communicate with LSM9DS1.\n");
jcallahan1 2:ae1bfb236387 10 }
simplyellow 4:ab08a70e0d7d 11 I->calibrate(1);
simplyellow 4:ab08a70e0d7d 12 //bool state;
simplyellow 4:ab08a70e0d7d 13 //Do we want to give state a starting value?
simplyellow 4:ab08a70e0d7d 14 ax01 = I->calcAccel(I->ax);
simplyellow 4:ab08a70e0d7d 15 ay01 = I->calcAccel(I->ay);
simplyellow 4:ab08a70e0d7d 16 az01 = I->calcAccel(I->az);
simplyellow 4:ab08a70e0d7d 17 ax0 = ax01;
simplyellow 4:ab08a70e0d7d 18 ay0 = ay01;
simplyellow 4:ab08a70e0d7d 19 az0 = az01;
jcallahan1 2:ae1bfb236387 20 }
simplyellow 4:ab08a70e0d7d 21
jcallahan1 2:ae1bfb236387 22 void IMU::trackBed()
jcallahan1 2:ae1bfb236387 23 {
jcallahan1 2:ae1bfb236387 24 if (state != 0) {
jcallahan1 2:ae1bfb236387 25 return;
jcallahan1 2:ae1bfb236387 26 }
jcallahan1 2:ae1bfb236387 27 else {
jcallahan1 2:ae1bfb236387 28 while(state == 0) {
simplyellow 4:ab08a70e0d7d 29 while(!I->accelAvailable());
simplyellow 4:ab08a70e0d7d 30 I->readAccel();
simplyellow 4:ab08a70e0d7d 31
simplyellow 4:ab08a70e0d7d 32 // current a = <ax,ay,az>
simplyellow 4:ab08a70e0d7d 33 ax = I->calcAccel(I->ax);
simplyellow 4:ab08a70e0d7d 34 ay = I->calcAccel(I->ay);
simplyellow 4:ab08a70e0d7d 35 az = I->calcAccel(I->az);
jcallahan1 2:ae1bfb236387 36 // theta in degrees between a0 and a
jcallahan1 2:ae1bfb236387 37 theta = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
jcallahan1 2:ae1bfb236387 38 }
jcallahan1 2:ae1bfb236387 39 }
jcallahan1 2:ae1bfb236387 40 }
simplyellow 4:ab08a70e0d7d 41
jcallahan1 2:ae1bfb236387 42 void IMU::trackHeading()
jcallahan1 2:ae1bfb236387 43 {
jcallahan1 2:ae1bfb236387 44 if (state != 1) {
jcallahan1 2:ae1bfb236387 45 return;
jcallahan1 2:ae1bfb236387 46 }
jcallahan1 2:ae1bfb236387 47 else {
jcallahan1 2:ae1bfb236387 48 while (state == 1) {
simplyellow 4:ab08a70e0d7d 49
simplyellow 4:ab08a70e0d7d 50 while(!I->accelAvailable());
simplyellow 4:ab08a70e0d7d 51
simplyellow 4:ab08a70e0d7d 52 I->readAccel();
simplyellow 4:ab08a70e0d7d 53 ax0 = I->calcAccel(I->ax);
simplyellow 4:ab08a70e0d7d 54 ay0 = I->calcAccel(I->ay);
simplyellow 4:ab08a70e0d7d 55 az0 = I->calcAccel(I->az);
jcallahan1 2:ae1bfb236387 56 }
jcallahan1 2:ae1bfb236387 57 }
jcallahan1 2:ae1bfb236387 58 }