Justin Gensel / Mbed 2 deprecated FinalProject

sadf

Dependencies:   LSM9DS1_Library_cal mbed XBee

Fork of FootSensor by Justin Gensel

main.cpp@3:2d6ff72599f1, 2017-04-24 (annotated)

Committer:
jgensel3
Date:
Mon Apr 24 04:05:46 2017 +0000
Revision:
3:2d6ff72599f1
Parent:
2:ab7b95fb52aa 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
4180_1 0:e693d5bf0a25 1 #include "mbed.h"
4180_1 0:e693d5bf0a25 2 #include "LSM9DS1.h"
jgensel3 3:2d6ff72599f1 3 #include "Wireless.h"
jgensel3 2:ab7b95fb52aa 4 //#include "USBKeyboard.h"
4180_1 0:e693d5bf0a25 5 #define PI 3.14159
4180_1 0:e693d5bf0a25 6 // Earth's magnetic field varies by location. Add or subtract
4180_1 0:e693d5bf0a25 7 // a declination to get a more accurate heading. Calculate
4180_1 0:e693d5bf0a25 8 // your's here:
4180_1 0:e693d5bf0a25 9 // http://www.ngdc.noaa.gov/geomag-web/#declination
4180_1 0:e693d5bf0a25 10 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
4180_1 0:e693d5bf0a25 11
jgensel3 2:ab7b95fb52aa 12 DigitalOut led1(LED1);
jgensel3 2:ab7b95fb52aa 13 DigitalOut led2(LED2);
jgensel3 2:ab7b95fb52aa 14 DigitalOut led3(LED3);
jgensel3 2:ab7b95fb52aa 15 DigitalOut led4(LED4);
4180_1 0:e693d5bf0a25 16 Serial pc(USBTX, USBRX);
jgensel3 3:2d6ff72599f1 17 DigitalIn pb1(p17);
jgensel3 2:ab7b95fb52aa 18 Timeout walkingTimer;
jgensel3 3:2d6ff72599f1 19 WirelessModule wireless(p9, p10, FOOT_STEP);
jgensel3 2:ab7b95fb52aa 20 bool isWalking = false;
4180_1 0:e693d5bf0a25 21 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 22 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 23 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 24 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 25 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
jgensel3 2:ab7b95fb52aa 26
jgensel3 2:ab7b95fb52aa 27
jgensel3 2:ab7b95fb52aa 28 //Function that timeout calls when detected that person stops walking
jgensel3 2:ab7b95fb52aa 29 void printStop()
jgensel3 2:ab7b95fb52aa 30 {
jgensel3 2:ab7b95fb52aa 31 // pc.printf("stop\n\r");
jgensel3 3:2d6ff72599f1 32 wireless.sendDirection(DIR_NONE);
jgensel3 2:ab7b95fb52aa 33 isWalking = false;
jgensel3 2:ab7b95fb52aa 34 }
jgensel3 2:ab7b95fb52aa 35
jgensel3 2:ab7b95fb52aa 36 //Rotates raw heading so 0 degrees is forward direction
jgensel3 2:ab7b95fb52aa 37 float correctHeading(float currHeading, float forward)
jgensel3 2:ab7b95fb52aa 38 {
jgensel3 1:705baf131710 39 float newHeading = currHeading - forward;
jgensel3 1:705baf131710 40 if(newHeading < 0) newHeading = 360 + newHeading;
jgensel3 1:705baf131710 41 return newHeading;
jgensel3 1:705baf131710 42 }
jgensel3 1:705baf131710 43
jgensel3 1:705baf131710 44 float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
4180_1 0:e693d5bf0a25 45 {
4180_1 0:e693d5bf0a25 46 float roll = atan2(ay, az);
4180_1 0:e693d5bf0a25 47 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
4180_1 0:e693d5bf0a25 48 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 49 mx = -mx;
4180_1 0:e693d5bf0a25 50 float heading;
4180_1 0:e693d5bf0a25 51 if (my == 0.0)
4180_1 0:e693d5bf0a25 52 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 53 else
4180_1 0:e693d5bf0a25 54 heading = atan2(mx, my)*360.0/(2.0*PI);
4180_1 0:e693d5bf0a25 55 //pc.printf("heading atan=%f \n\r",heading);
4180_1 0:e693d5bf0a25 56 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 57 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 58 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 59 else if(heading<0.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 60
4180_1 0:e693d5bf0a25 61
4180_1 0:e693d5bf0a25 62 // Convert everything from radians to degrees:
4180_1 0:e693d5bf0a25 63 //heading *= 180.0 / PI;
4180_1 0:e693d5bf0a25 64 pitch *= 180.0 / PI;
4180_1 0:e693d5bf0a25 65 roll *= 180.0 / PI;
4180_1 0:e693d5bf0a25 66
jgensel3 1:705baf131710 67 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jgensel3 1:705baf131710 68 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
jgensel3 1:705baf131710 69 return heading;
4180_1 0:e693d5bf0a25 70 }
4180_1 0:e693d5bf0a25 71
4180_1 0:e693d5bf0a25 72
4180_1 0:e693d5bf0a25 73
4180_1 0:e693d5bf0a25 74
4180_1 0:e693d5bf0a25 75 int main()
4180_1 0:e693d5bf0a25 76 {
4180_1 0:e693d5bf0a25 77 //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
jgensel3 3:2d6ff72599f1 78 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
jgensel3 1:705baf131710 79 pb1.mode(PullUp);
4180_1 0:e693d5bf0a25 80 IMU.begin();
jgensel3 1:705baf131710 81 float forward;
4180_1 0:e693d5bf0a25 82 if (!IMU.begin()) {
4180_1 0:e693d5bf0a25 83 pc.printf("Failed to communicate with LSM9DS1.\n");
4180_1 0:e693d5bf0a25 84 }
jgensel3 2:ab7b95fb52aa 85 led3 = 1;
jgensel3 2:ab7b95fb52aa 86 wait(1);
4180_1 0:e693d5bf0a25 87 IMU.calibrate(1);
jgensel3 2:ab7b95fb52aa 88 led3 = 0;
jgensel3 2:ab7b95fb52aa 89 led1 = 1;
jgensel3 2:ab7b95fb52aa 90 wait(0.5);
jgensel3 2:ab7b95fb52aa 91 led4 = 1;
jgensel3 2:ab7b95fb52aa 92 wait(0.5);
4180_1 0:e693d5bf0a25 93 IMU.calibrateMag(0);
jgensel3 2:ab7b95fb52aa 94 led4 = 0;
jgensel3 2:ab7b95fb52aa 95 led2 = 1;
jgensel3 2:ab7b95fb52aa 96
jgensel3 2:ab7b95fb52aa 97 //Get forward direction relative to user
jgensel3 1:705baf131710 98 pc.printf("Press button to set forward direction");
jgensel3 2:ab7b95fb52aa 99 while(pb1 == 1) {
jgensel3 1:705baf131710 100 IMU.readMag();
jgensel3 1:705baf131710 101 IMU.readAccel();
jgensel3 1:705baf131710 102 forward = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jgensel3 2:ab7b95fb52aa 103 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));;
jgensel3 2:ab7b95fb52aa 104 }
jgensel3 2:ab7b95fb52aa 105 pc.printf("forward: %f\n\r", forward);
4180_1 0:e693d5bf0a25 106 while(1) {
4180_1 0:e693d5bf0a25 107 while(!IMU.tempAvailable());
4180_1 0:e693d5bf0a25 108 IMU.readTemp();
4180_1 0:e693d5bf0a25 109 while(!IMU.magAvailable(X_AXIS));
4180_1 0:e693d5bf0a25 110 IMU.readMag();
4180_1 0:e693d5bf0a25 111 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 112 IMU.readAccel();
4180_1 0:e693d5bf0a25 113 while(!IMU.gyroAvailable());
4180_1 0:e693d5bf0a25 114 IMU.readGyro();
jgensel3 2:ab7b95fb52aa 115 if(abs(IMU.calcGyro(IMU.gy)) > 100) {
jgensel3 2:ab7b95fb52aa 116
jgensel3 2:ab7b95fb52aa 117
jgensel3 2:ab7b95fb52aa 118 IMU.readAccel();
jgensel3 2:ab7b95fb52aa 119
jgensel3 2:ab7b95fb52aa 120 IMU.readMag();
jgensel3 2:ab7b95fb52aa 121
jgensel3 2:ab7b95fb52aa 122
jgensel3 2:ab7b95fb52aa 123 //Calculate heading relative to forward direction
jgensel3 1:705baf131710 124 float currHeading = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jgensel3 1:705baf131710 125 currHeading = correctHeading(currHeading, forward) + 45;
jgensel3 2:ab7b95fb52aa 126 //Start timeout to detect when stopped walking
jgensel3 2:ab7b95fb52aa 127 walkingTimer.attach(printStop, 0.3);
jgensel3 2:ab7b95fb52aa 128
jgensel3 2:ab7b95fb52aa 129 //Detect direction and send command to main mbed
jgensel3 2:ab7b95fb52aa 130 if((currHeading > 270 && currHeading < 360) && !isWalking) {
jgensel3 2:ab7b95fb52aa 131
jgensel3 3:2d6ff72599f1 132 wireless.sendDirection(DIR_LEFT);
jgensel3 2:ab7b95fb52aa 133
jgensel3 2:ab7b95fb52aa 134 isWalking = true;
jgensel3 2:ab7b95fb52aa 135 } else if((currHeading > 90 && currHeading < 180) && !isWalking) {
jgensel3 2:ab7b95fb52aa 136
jgensel3 3:2d6ff72599f1 137 wireless.sendDirection(DIR_RIGHT);
jgensel3 2:ab7b95fb52aa 138 isWalking = true;
jgensel3 2:ab7b95fb52aa 139 } else if((currHeading > 180 && currHeading < 270) && !isWalking) {
jgensel3 2:ab7b95fb52aa 140
jgensel3 3:2d6ff72599f1 141 wireless.sendDirection(DIR_DOWN);
jgensel3 2:ab7b95fb52aa 142 isWalking = true;
jgensel3 2:ab7b95fb52aa 143 } else if((currHeading > 360 || currHeading < 90) && !isWalking) {
jgensel3 2:ab7b95fb52aa 144
jgensel3 3:2d6ff72599f1 145 wireless.sendDirection(DIR_UP);
jgensel3 2:ab7b95fb52aa 146 isWalking = true;
jgensel3 1:705baf131710 147 }
jgensel3 1:705baf131710 148 }
4180_1 0:e693d5bf0a25 149 }
4180_1 0:e693d5bf0a25 150 }
4180_1 0:e693d5bf0a25 151