Justin Gensel / Mbed 2 deprecated FootModuleCode

Code to detect walking and convert to input for video game

Dependencies:   LSM9DS1_Library_cal2 XBee mbed

Fork of FootModule by Justin Gensel

main.cpp@7:7694edb02dd3, 2017-05-01 (annotated)

Committer:
jgensel3
Date:
Mon May 01 12:44:19 2017 +0000
Revision:
7:7694edb02dd3
Parent:
5:c4ae0656a736 
deleted redundant code

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