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@5:665b9dc3d22f, 2016-04-26 (annotated)

Committer:
leeeang
Date:
Tue Apr 26 18:25:30 2016 +0000
Revision:
5:665b9dc3d22f
Parent:
4:18e9f16cc53c 
Added Motor driver at the bottom;

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 4:18e9f16cc53c 60 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 61 Thread::wait(500); //Wait 1/2 second
wschon 3:70f624c5fe26 62 }
wschon 3:70f624c5fe26 63 }
wschon 0:8e17f11689f2 64
wschon 2:fcf6f5901ee6 65 void left_thread(void const *args) {
wschon 2:fcf6f5901ee6 66 while (1) {
wschon 2:fcf6f5901ee6 67 leftDist = infraredL * 3.3f;
wschon 2:fcf6f5901ee6 68 Thread::wait(500); //wait 1/2 second before updating
wschon 2:fcf6f5901ee6 69 }
wschon 2:fcf6f5901ee6 70 }
wschon 2:fcf6f5901ee6 71
wschon 2:fcf6f5901ee6 72 void right_thread(void const *args) {
wschon 2:fcf6f5901ee6 73 while (1) {
wschon 2:fcf6f5901ee6 74 rightDist = infraredR * 3.3f;
wschon 2:fcf6f5901ee6 75 Thread::wait(500); //wait 1/2 second before updating
wschon 2:fcf6f5901ee6 76 }
wschon 2:fcf6f5901ee6 77 }
wschon 2:fcf6f5901ee6 78
wschon 2:fcf6f5901ee6 79 void front_thread(void const *args) {
wschon 2:fcf6f5901ee6 80 while (1) {
wschon 2:fcf6f5901ee6 81 frontDist = infraredF * 3.3f;
wschon 2:fcf6f5901ee6 82 Thread::wait(500); //wait 1/2 second before updating
wschon 2:fcf6f5901ee6 83 }
wschon 2:fcf6f5901ee6 84 }
wschon 0:8e17f11689f2 85
wschon 0:8e17f11689f2 86 int main() {
swilkins8 1:27c32ba9fe93 87 //Test
wschon 2:fcf6f5901ee6 88 Thread left(left_thread);
wschon 2:fcf6f5901ee6 89 Thread right(right_thread);
wschon 2:fcf6f5901ee6 90 Thread front(front_thread);
wschon 2:fcf6f5901ee6 91
wschon 0:8e17f11689f2 92 while(1) {
wschon 2:fcf6f5901ee6 93
wschon 0:8e17f11689f2 94 }
wschon 0:8e17f11689f2 95 }
leeeang 5:665b9dc3d22f 96
leeeang 5:665b9dc3d22f 97
leeeang 5:665b9dc3d22f 98 //driving robot
leeeang 5:665b9dc3d22f 99 /*
leeeang 5:665b9dc3d22f 100
leeeang 5:665b9dc3d22f 101 #include "mbed.h"
leeeang 5:665b9dc3d22f 102
leeeang 5:665b9dc3d22f 103 //dual H bridge Polulu
leeeang 5:665b9dc3d22f 104 PwmOut PWMA(p21); //connect p21 to PWMA
leeeang 5:665b9dc3d22f 105 PwmOut PWMB(p22); //connect p22 to PWMB
leeeang 5:665b9dc3d22f 106 DigitalOut Ain1(p23); //connect p23 to Ain1
leeeang 5:665b9dc3d22f 107 DigitalOut Ain2(p24); //connect p24 to Ain2
leeeang 5:665b9dc3d22f 108
leeeang 5:665b9dc3d22f 109 int main() {
leeeang 5:665b9dc3d22f 110 Ain1=Ain2=1; //enabling PWM
leeeang 5:665b9dc3d22f 111 while(1) {
leeeang 5:665b9dc3d22f 112 PWMA.write(0.5f); //writing 50% duty cycle
leeeang 5:665b9dc3d22f 113 PWMB.write(0.5f); //to pwm A and B
leeeang 5:665b9dc3d22f 114 }
leeeang 5:665b9dc3d22f 115 }
leeeang 5:665b9dc3d22f 116
leeeang 5:665b9dc3d22f 117 */