Wesley Schon / Mbed 2 deprecated ECE4180_Robot

Robot using IMU and IR sensor data to drive autonomously and create a map

Dependencies:   mbed mbed-rtos LSM9DS1_Library_cal Motor

main.cpp@3:70f624c5fe26, 2016-04-10 (annotated)

Committer:
wschon
Date:
Sun Apr 10 22:01:21 2016 +0000
Revision:
3:70f624c5fe26
Parent:
2:fcf6f5901ee6
Child:
4:18e9f16cc53c
added magnetic heading and accelerometer code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wschon 0:8e17f11689f2 1 #include "mbed.h"
wschon 2:fcf6f5901ee6 2 #include "rtos.h"
wschon 3:70f624c5fe26 3 #include "LSM9DS1.h"
wschon 3:70f624c5fe26 4 #define DECLINATION -4.94
wschon 3:70f624c5fe26 5 #define PI 3.14159
wschon 3:70f624c5fe26 6
wschon 2:fcf6f5901ee6 7
wschon 2:fcf6f5901ee6 8 AnalogIn infraredR(p20); //Right infrared distance sensor
wschon 2:fcf6f5901ee6 9 AnalogIn infraredL(p19); //Left infrared distance sensor
wschon 2:fcf6f5901ee6 10 AnalogIn infraredF(p17); //Front infrared distance sensor
wschon 3:70f624c5fe26 11 Serial pc(USBTX, USBRX);
wschon 2:fcf6f5901ee6 12
wschon 2:fcf6f5901ee6 13 float leftDist, rightDist, frontDist; //Distances from robot to obstacles
wschon 3:70f624c5fe26 14 float xaccel, yaccel, zaccel; //Acceleration in the x, y, and z directions
wschon 3:70f624c5fe26 15 float xmag, ymag, zmag, head; //Magnetic readings
wschon 3:70f624c5fe26 16
wschon 3:70f624c5fe26 17 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
wschon 3:70f624c5fe26 18 {
wschon 3:70f624c5fe26 19 float roll = atan2(ay, az);
wschon 3:70f624c5fe26 20 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
wschon 3:70f624c5fe26 21 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
wschon 3:70f624c5fe26 22 mx = -mx;
wschon 3:70f624c5fe26 23 float heading;
wschon 3:70f624c5fe26 24 if (my == 0.0)
wschon 3:70f624c5fe26 25 heading = (mx < 0.0) ? 180.0 : 0.0;
wschon 3:70f624c5fe26 26 else
wschon 3:70f624c5fe26 27 heading = atan2(mx, my)*360.0/(2.0*PI);
wschon 3:70f624c5fe26 28 //pc.printf("heading atan=%f \n\r",heading);
wschon 3:70f624c5fe26 29 heading -= DECLINATION; //correct for geo location
wschon 3:70f624c5fe26 30 if(heading>180.0) heading = heading - 360.0;
wschon 3:70f624c5fe26 31 else if(heading<-180.0) heading = 360.0 + heading;
wschon 3:70f624c5fe26 32 else if(heading<0.0) heading = 360.0 + heading;
wschon 3:70f624c5fe26 33
wschon 3:70f624c5fe26 34
wschon 3:70f624c5fe26 35 // Convert everything from radians to degrees:
wschon 3:70f624c5fe26 36 //heading *= 180.0 / PI;
wschon 3:70f624c5fe26 37 pitch *= 180.0 / PI;
wschon 3:70f624c5fe26 38 roll *= 180.0 / PI;
wschon 3:70f624c5fe26 39
wschon 3:70f624c5fe26 40 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
wschon 3:70f624c5fe26 41 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
wschon 3:70f624c5fe26 42 head = heading;
wschon 3:70f624c5fe26 43 }
wschon 3:70f624c5fe26 44
wschon 3:70f624c5fe26 45 void IMU_thread(void const *args) {
wschon 3:70f624c5fe26 46 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
wschon 3:70f624c5fe26 47 IMU.calibrate(1);
wschon 3:70f624c5fe26 48 IMU.calibrateMag(0);
wschon 3:70f624c5fe26 49 while(1) {
wschon 3:70f624c5fe26 50 while(!IMU.accelAvailable());
wschon 3:70f624c5fe26 51 IMU.readAccel();
wschon 3:70f624c5fe26 52 while(!IMU.magAvailable(X_AXIS));
wschon 3:70f624c5fe26 53 IMU.readMag();
wschon 3:70f624c5fe26 54 xaccel = IMU.calcAccel(IMU.ax);
wschon 3:70f624c5fe26 55 yaccel = IMU.calcAccel(IMU.ay);
wschon 3:70f624c5fe26 56 zaccel = IMU.calcAccel(IMU.az);
wschon 3:70f624c5fe26 57 xmag = IMU.calcMag(IMU.mx);
wschon 3:70f624c5fe26 58 ymag = IMU.calcMag(IMU.my);
wschon 3:70f624c5fe26 59 zmag = IMU.calcMag(IMU.mz);
wschon 3:70f624c5fe26 60 Thread::wait(500); //Wait 1/2 second
wschon 3:70f624c5fe26 61 }
wschon 3:70f624c5fe26 62 }
wschon 0:8e17f11689f2 63
wschon 2:fcf6f5901ee6 64 void left_thread(void const *args) {
wschon 2:fcf6f5901ee6 65 while (1) {
wschon 2:fcf6f5901ee6 66 leftDist = infraredL * 3.3f;
wschon 2:fcf6f5901ee6 67 Thread::wait(500); //wait 1/2 second before updating
wschon 2:fcf6f5901ee6 68 }
wschon 2:fcf6f5901ee6 69 }
wschon 2:fcf6f5901ee6 70
wschon 2:fcf6f5901ee6 71 void right_thread(void const *args) {
wschon 2:fcf6f5901ee6 72 while (1) {
wschon 2:fcf6f5901ee6 73 rightDist = infraredR * 3.3f;
wschon 2:fcf6f5901ee6 74 Thread::wait(500); //wait 1/2 second before updating
wschon 2:fcf6f5901ee6 75 }
wschon 2:fcf6f5901ee6 76 }
wschon 2:fcf6f5901ee6 77
wschon 2:fcf6f5901ee6 78 void front_thread(void const *args) {
wschon 2:fcf6f5901ee6 79 while (1) {
wschon 2:fcf6f5901ee6 80 frontDist = infraredF * 3.3f;
wschon 2:fcf6f5901ee6 81 Thread::wait(500); //wait 1/2 second before updating
wschon 2:fcf6f5901ee6 82 }
wschon 2:fcf6f5901ee6 83 }
wschon 0:8e17f11689f2 84
wschon 0:8e17f11689f2 85 int main() {
swilkins8 1:27c32ba9fe93 86 //Test
wschon 2:fcf6f5901ee6 87 Thread left(left_thread);
wschon 2:fcf6f5901ee6 88 Thread right(right_thread);
wschon 2:fcf6f5901ee6 89 Thread front(front_thread);
wschon 2:fcf6f5901ee6 90
wschon 0:8e17f11689f2 91 while(1) {
wschon 2:fcf6f5901ee6 92
wschon 0:8e17f11689f2 93 }
wschon 0:8e17f11689f2 94 }