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@7:cbccf5c5da6d, 2016-04-28 (annotated)

Committer:
wschon
Date:
Thu Apr 28 05:20:35 2016 +0000
Revision:
7:cbccf5c5da6d
Parent:
6:a51f4ac6f42b
Child:
8:198568d5746b
added motor

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 6:a51f4ac6f42b 7 // Timer example: https://developer.mbed.org/handbook/Timer
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 6:a51f4ac6f42b 12 Timer t;
wschon 7:cbccf5c5da6d 13 char str[40];
wschon 6:a51f4ac6f42b 14
wschon 2:fcf6f5901ee6 15 float leftDist, rightDist, frontDist; //Distances from robot to obstacles
wschon 3:70f624c5fe26 16 float xaccel, yaccel, zaccel; //Acceleration in the x, y, and z directions
wschon 3:70f624c5fe26 17 float xmag, ymag, zmag, head; //Magnetic readings
wschon 6:a51f4ac6f42b 18 int ldist, rdist, fdist;
wschon 3:70f624c5fe26 19
wschon 7:cbccf5c5da6d 20 //dual H bridge Polulu
wschon 7:cbccf5c5da6d 21 PwmOut PWMA(p21); //connect p21 to PWMA
wschon 7:cbccf5c5da6d 22 PwmOut PWMB(p22); //connect p22 to PWMB
wschon 7:cbccf5c5da6d 23 DigitalOut Ain1(p23); //connect p23 to Ain1
wschon 7:cbccf5c5da6d 24 DigitalOut Ain2(p24); //connect p24 to Ain2
wschon 7:cbccf5c5da6d 25
wschon 7:cbccf5c5da6d 26
wschon 7:cbccf5c5da6d 27
wschon 7:cbccf5c5da6d 28
wschon 3:70f624c5fe26 29 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
wschon 3:70f624c5fe26 30 {
wschon 3:70f624c5fe26 31 float roll = atan2(ay, az);
wschon 3:70f624c5fe26 32 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
wschon 3:70f624c5fe26 33 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
wschon 3:70f624c5fe26 34 mx = -mx;
wschon 3:70f624c5fe26 35 float heading;
wschon 3:70f624c5fe26 36 if (my == 0.0)
wschon 3:70f624c5fe26 37 heading = (mx < 0.0) ? 180.0 : 0.0;
wschon 3:70f624c5fe26 38 else
wschon 3:70f624c5fe26 39 heading = atan2(mx, my)*360.0/(2.0*PI);
wschon 3:70f624c5fe26 40 //pc.printf("heading atan=%f \n\r",heading);
wschon 3:70f624c5fe26 41 heading -= DECLINATION; //correct for geo location
wschon 3:70f624c5fe26 42 if(heading>180.0) heading = heading - 360.0;
wschon 3:70f624c5fe26 43 else if(heading<-180.0) heading = 360.0 + heading;
wschon 3:70f624c5fe26 44 else if(heading<0.0) heading = 360.0 + heading;
wschon 3:70f624c5fe26 45
wschon 3:70f624c5fe26 46
wschon 3:70f624c5fe26 47 // Convert everything from radians to degrees:
wschon 3:70f624c5fe26 48 //heading *= 180.0 / PI;
wschon 3:70f624c5fe26 49 pitch *= 180.0 / PI;
wschon 3:70f624c5fe26 50 roll *= 180.0 / PI;
wschon 3:70f624c5fe26 51
wschon 3:70f624c5fe26 52 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
wschon 3:70f624c5fe26 53 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
wschon 3:70f624c5fe26 54 head = heading;
wschon 3:70f624c5fe26 55 }
wschon 3:70f624c5fe26 56
wschon 3:70f624c5fe26 57 void IMU_thread(void const *args) {
wschon 3:70f624c5fe26 58 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
wschon 6:a51f4ac6f42b 59 IMU.begin();
wschon 6:a51f4ac6f42b 60 if (!IMU.begin()) {
wschon 6:a51f4ac6f42b 61 pc.printf("Failed to communicate with LSM9DS1.\n");
wschon 6:a51f4ac6f42b 62 }
wschon 3:70f624c5fe26 63 IMU.calibrate(1);
wschon 3:70f624c5fe26 64 IMU.calibrateMag(0);
wschon 3:70f624c5fe26 65 while(1) {
wschon 3:70f624c5fe26 66 while(!IMU.accelAvailable());
wschon 3:70f624c5fe26 67 IMU.readAccel();
wschon 3:70f624c5fe26 68 while(!IMU.magAvailable(X_AXIS));
wschon 3:70f624c5fe26 69 IMU.readMag();
wschon 3:70f624c5fe26 70 xaccel = IMU.calcAccel(IMU.ax);
wschon 3:70f624c5fe26 71 yaccel = IMU.calcAccel(IMU.ay);
wschon 3:70f624c5fe26 72 zaccel = IMU.calcAccel(IMU.az);
wschon 6:a51f4ac6f42b 73 pc.printf("%f\r\n",zaccel);
wschon 3:70f624c5fe26 74 xmag = IMU.calcMag(IMU.mx);
wschon 3:70f624c5fe26 75 ymag = IMU.calcMag(IMU.my);
wschon 3:70f624c5fe26 76 zmag = IMU.calcMag(IMU.mz);
wschon 4:18e9f16cc53c 77 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));
wschon 3:70f624c5fe26 78 Thread::wait(500); //Wait 1/2 second
wschon 3:70f624c5fe26 79 }
wschon 3:70f624c5fe26 80 }
wschon 0:8e17f11689f2 81
wschon 2:fcf6f5901ee6 82 void left_thread(void const *args) {
wschon 2:fcf6f5901ee6 83 while (1) {
wschon 6:a51f4ac6f42b 84 leftDist = infraredL * 80.0f;
wschon 6:a51f4ac6f42b 85 leftDist = 80.0f - leftDist;
wschon 6:a51f4ac6f42b 86 ldist = (int)leftDist;
wschon 2:fcf6f5901ee6 87 Thread::wait(500); //wait 1/2 second before updating
wschon 6:a51f4ac6f42b 88 //pc.printf("Left distance %d\r\n", ldist);
wschon 2:fcf6f5901ee6 89 }
wschon 2:fcf6f5901ee6 90 }
wschon 2:fcf6f5901ee6 91
wschon 2:fcf6f5901ee6 92 void right_thread(void const *args) {
wschon 2:fcf6f5901ee6 93 while (1) {
wschon 6:a51f4ac6f42b 94 rightDist = infraredR * 80.0f;
wschon 6:a51f4ac6f42b 95 rightDist = 80.0f - rightDist;
wschon 6:a51f4ac6f42b 96 rdist = (int)rightDist;
wschon 6:a51f4ac6f42b 97 rdist = (int)rightDist;
wschon 2:fcf6f5901ee6 98 Thread::wait(500); //wait 1/2 second before updating
wschon 6:a51f4ac6f42b 99 //pc.printf("Right distance %d\r\n", rdist);
wschon 2:fcf6f5901ee6 100 }
wschon 2:fcf6f5901ee6 101 }
wschon 2:fcf6f5901ee6 102
wschon 2:fcf6f5901ee6 103 void front_thread(void const *args) {
wschon 2:fcf6f5901ee6 104 while (1) {
wschon 6:a51f4ac6f42b 105 frontDist = infraredF * 80.0f;
wschon 6:a51f4ac6f42b 106 frontDist = 80.0f - frontDist;
wschon 6:a51f4ac6f42b 107 fdist = (int)frontDist;
wschon 2:fcf6f5901ee6 108 Thread::wait(500); //wait 1/2 second before updating
wschon 6:a51f4ac6f42b 109 pc.printf("Front distance %d\r\n", fdist);
wschon 2:fcf6f5901ee6 110 }
wschon 2:fcf6f5901ee6 111 }
wschon 0:8e17f11689f2 112
wschon 7:cbccf5c5da6d 113 void send_thread(void const *args) {
wschon 7:cbccf5c5da6d 114 while (1) {
wschon 7:cbccf5c5da6d 115 sprintf (str, "%0.6f,%0.6f,%0.6f,%d,%d,%d,%d\n", xaccel, zaccel, ymag, fdist, ldist, rdist, t.read_us()); //buffer data to be sent to computer
wschon 7:cbccf5c5da6d 116 Thread::wait(500);
wschon 7:cbccf5c5da6d 117 }
wschon 7:cbccf5c5da6d 118 }
wschon 7:cbccf5c5da6d 119
wschon 7:cbccf5c5da6d 120
wschon 0:8e17f11689f2 121 int main() {
swilkins8 1:27c32ba9fe93 122 //Test
wschon 7:cbccf5c5da6d 123 t.start();
wschon 2:fcf6f5901ee6 124 Thread left(left_thread);
wschon 2:fcf6f5901ee6 125 Thread right(right_thread);
wschon 2:fcf6f5901ee6 126 Thread front(front_thread);
wschon 6:a51f4ac6f42b 127 Thread IMU(IMU_thread);
wschon 7:cbccf5c5da6d 128 Thread send(send_thread);
wschon 7:cbccf5c5da6d 129 Ain1=Ain2=1; //enabling PWM
wschon 0:8e17f11689f2 130 while(1) {
wschon 7:cbccf5c5da6d 131 PWMA.write(0.5f); //writing 50% duty cycle
wschon 7:cbccf5c5da6d 132 PWMB.write(0.5f); //to pwm A and B
wschon 0:8e17f11689f2 133 }
wschon 0:8e17f11689f2 134 }