James Plager / Mbed 2 deprecated IMURoomba4_ThrowSumMo

ECE4180 Final Project

Dependencies:   LSM9DS1_Library Motor mbed-rtos mbed

Fork of IMURoomba4_ThrowSumMo by James Plager

main.cpp@0:c29fc80c3ca3, 2017-05-02 (annotated)

Committer:
jplager3
Date:
Tue May 02 19:22:12 2017 +0000
Revision:
0:c29fc80c3ca3
Child:
1:6b8a201f4f90
Setup good, no compile errors. Warnings aplenty. Drive functionality in progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jplager3 0:c29fc80c3ca3 1 #include "mbed.h"
jplager3 0:c29fc80c3ca3 2 #include "LSM9DS1.h"
jplager3 0:c29fc80c3ca3 3 #include "rtos.h"
jplager3 0:c29fc80c3ca3 4 //#include "SDFileSystem.h"
jplager3 0:c29fc80c3ca3 5 #include "Motor.h"
jplager3 0:c29fc80c3ca3 6 //#include "wave_player.h"
jplager3 0:c29fc80c3ca3 7
jplager3 0:c29fc80c3ca3 8 #define PI 3.14159
jplager3 0:c29fc80c3ca3 9 // Earth's magnetic field varies by location. Add or subtract
jplager3 0:c29fc80c3ca3 10 // a declination to get a more accurate heading. Calculate
jplager3 0:c29fc80c3ca3 11 // your's here:
jplager3 0:c29fc80c3ca3 12 // http://www.ngdc.noaa.gov/geomag-web/#declination
jplager3 0:c29fc80c3ca3 13 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
jplager3 0:c29fc80c3ca3 14 //collab test
jplager3 0:c29fc80c3ca3 15
jplager3 0:c29fc80c3ca3 16 Serial pc(USBTX, USBRX);
jplager3 0:c29fc80c3ca3 17 //RawSerial pc(USBTX, USBRX);
jplager3 0:c29fc80c3ca3 18 Serial dev(p28,p27); //
jplager3 0:c29fc80c3ca3 19 //RawSerial dev(p28,p27); //tx, rx
jplager3 0:c29fc80c3ca3 20 DigitalOut myled(LED1);
jplager3 0:c29fc80c3ca3 21 DigitalOut led2(LED2);
jplager3 0:c29fc80c3ca3 22 DigitalOut led4(LED4);
jplager3 0:c29fc80c3ca3 23 //IR sensors on p19(front) & p20 (right)
jplager3 0:c29fc80c3ca3 24 AnalogIn IR1(p19);
jplager3 0:c29fc80c3ca3 25 AnalogIn IR2(p20);
jplager3 0:c29fc80c3ca3 26 //L and R DC motors
jplager3 0:c29fc80c3ca3 27 Motor Left(p21, p14, p13); // green wires. pwm, fwd, rev, add ", 1" for braking
jplager3 0:c29fc80c3ca3 28 Motor Right(p22, p12, p11); // red wires
jplager3 0:c29fc80c3ca3 29 // Speaker out
jplager3 0:c29fc80c3ca3 30 AnalogOut DACout(p18); //must(?) be p18
jplager3 0:c29fc80c3ca3 31 //SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
jplager3 0:c29fc80c3ca3 32
jplager3 0:c29fc80c3ca3 33
jplager3 0:c29fc80c3ca3 34 Thread thread1;
jplager3 0:c29fc80c3ca3 35 Thread thread2;
jplager3 0:c29fc80c3ca3 36 Mutex BTmutex;
jplager3 0:c29fc80c3ca3 37 Mutex mutex;
jplager3 0:c29fc80c3ca3 38
jplager3 0:c29fc80c3ca3 39
jplager3 0:c29fc80c3ca3 40 // Calculate pitch, roll, and heading.
jplager3 0:c29fc80c3ca3 41 // Pitch/roll calculations taken from this app note:
jplager3 0:c29fc80c3ca3 42 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
jplager3 0:c29fc80c3ca3 43 // Heading calculations taken from this app note:
jplager3 0:c29fc80c3ca3 44 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
jplager3 0:c29fc80c3ca3 45 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
jplager3 0:c29fc80c3ca3 46 {
jplager3 0:c29fc80c3ca3 47 float roll = atan2(ay, az);
jplager3 0:c29fc80c3ca3 48 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jplager3 0:c29fc80c3ca3 49 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jplager3 0:c29fc80c3ca3 50 mx = -mx;
jplager3 0:c29fc80c3ca3 51 float heading;
jplager3 0:c29fc80c3ca3 52 if (my == 0.0)
jplager3 0:c29fc80c3ca3 53 heading = (mx < 0.0) ? 180.0 : 0.0;
jplager3 0:c29fc80c3ca3 54 else
jplager3 0:c29fc80c3ca3 55 heading = atan2(mx, my)*360.0/(2.0*PI);
jplager3 0:c29fc80c3ca3 56 //pc.printf("heading atan=%f \n\r",heading);
jplager3 0:c29fc80c3ca3 57 heading -= DECLINATION; //correct for geo location
jplager3 0:c29fc80c3ca3 58 if(heading>180.0) heading = heading - 360.0;
jplager3 0:c29fc80c3ca3 59 else if(heading<-180.0) heading = 360.0 + heading;
jplager3 0:c29fc80c3ca3 60 else if(heading<0.0) heading = 360.0 + heading;
jplager3 0:c29fc80c3ca3 61 // Convert everything from radians to degrees:
jplager3 0:c29fc80c3ca3 62 //heading *= 180.0 / PI;
jplager3 0:c29fc80c3ca3 63 pitch *= 180.0 / PI;
jplager3 0:c29fc80c3ca3 64 roll *= 180.0 / PI;
jplager3 0:c29fc80c3ca3 65 //~pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jplager3 0:c29fc80c3ca3 66 //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
jplager3 0:c29fc80c3ca3 67 }
jplager3 0:c29fc80c3ca3 68
jplager3 0:c29fc80c3ca3 69 /*
jplager3 0:c29fc80c3ca3 70 void dev_recv()
jplager3 0:c29fc80c3ca3 71 {
jplager3 0:c29fc80c3ca3 72 led2 = !led2;
jplager3 0:c29fc80c3ca3 73 while(dev.readable()) {
jplager3 0:c29fc80c3ca3 74 pc.putc(dev.getc());
jplager3 0:c29fc80c3ca3 75 }
jplager3 0:c29fc80c3ca3 76 }
jplager3 0:c29fc80c3ca3 77
jplager3 0:c29fc80c3ca3 78 void pc_recv()
jplager3 0:c29fc80c3ca3 79 {
jplager3 0:c29fc80c3ca3 80 led4 = !led4;
jplager3 0:c29fc80c3ca3 81 while(pc.readable()) {
jplager3 0:c29fc80c3ca3 82 dev.putc(pc.getc());
jplager3 0:c29fc80c3ca3 83 }
jplager3 0:c29fc80c3ca3 84 }*/
jplager3 0:c29fc80c3ca3 85
jplager3 0:c29fc80c3ca3 86 // Driving Methods
jplager3 0:c29fc80c3ca3 87 void forward(float speed){
jplager3 0:c29fc80c3ca3 88 Left.speed(speed);
jplager3 0:c29fc80c3ca3 89 Right.speed(speed);
jplager3 0:c29fc80c3ca3 90 }
jplager3 0:c29fc80c3ca3 91 void reverse(float speed){
jplager3 0:c29fc80c3ca3 92 Left.speed(-speed);
jplager3 0:c29fc80c3ca3 93 Right.speed(-speed);
jplager3 0:c29fc80c3ca3 94 }
jplager3 0:c29fc80c3ca3 95 void turnRight(float speed){
jplager3 0:c29fc80c3ca3 96 Left.speed(speed);
jplager3 0:c29fc80c3ca3 97 Right.speed(-speed);
jplager3 0:c29fc80c3ca3 98 //wait(0.7);
jplager3 0:c29fc80c3ca3 99 }
jplager3 0:c29fc80c3ca3 100 void turnLeft(float speed){
jplager3 0:c29fc80c3ca3 101 Left.speed(-speed);
jplager3 0:c29fc80c3ca3 102 Right.speed(speed);
jplager3 0:c29fc80c3ca3 103 //wait(0.7);
jplager3 0:c29fc80c3ca3 104 }
jplager3 0:c29fc80c3ca3 105 void stop(){
jplager3 0:c29fc80c3ca3 106 Left.speed(0.0);
jplager3 0:c29fc80c3ca3 107 Right.speed(0.0);
jplager3 0:c29fc80c3ca3 108 }
jplager3 0:c29fc80c3ca3 109
jplager3 0:c29fc80c3ca3 110 void IMU(){
jplager3 0:c29fc80c3ca3 111 //IMU setup
jplager3 0:c29fc80c3ca3 112 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); // this executes. Pins are correct. Changing them causes fault
jplager3 0:c29fc80c3ca3 113 IMU.begin();
jplager3 0:c29fc80c3ca3 114 if (!IMU.begin()) {
jplager3 0:c29fc80c3ca3 115 led2=1;
jplager3 0:c29fc80c3ca3 116 pc.printf("Failed to communicate with LSM9DS1.\n");
jplager3 0:c29fc80c3ca3 117 }
jplager3 0:c29fc80c3ca3 118 IMU.calibrate(1);
jplager3 0:c29fc80c3ca3 119 IMU.calibrateMag(0);
jplager3 0:c29fc80c3ca3 120
jplager3 0:c29fc80c3ca3 121 //bluetooth setup
jplager3 0:c29fc80c3ca3 122 pc.baud(9600);
jplager3 0:c29fc80c3ca3 123 dev.baud(9600);
jplager3 0:c29fc80c3ca3 124
jplager3 0:c29fc80c3ca3 125 /*pc.attach(&pc_recv, Serial::RxIrq);
jplager3 0:c29fc80c3ca3 126 dev.attach(&dev_recv, Serial::RxIrq);*/
jplager3 0:c29fc80c3ca3 127
jplager3 0:c29fc80c3ca3 128 while(1) {
jplager3 0:c29fc80c3ca3 129 //myled = 1;
jplager3 0:c29fc80c3ca3 130 while(!IMU.magAvailable(X_AXIS));
jplager3 0:c29fc80c3ca3 131 IMU.readMag();
jplager3 0:c29fc80c3ca3 132 //myled = 0;
jplager3 0:c29fc80c3ca3 133 while(!IMU.accelAvailable());
jplager3 0:c29fc80c3ca3 134 IMU.readAccel();
jplager3 0:c29fc80c3ca3 135 while(!IMU.gyroAvailable());
jplager3 0:c29fc80c3ca3 136 IMU.readGyro();
jplager3 0:c29fc80c3ca3 137 BTmutex.lock();
jplager3 0:c29fc80c3ca3 138 pc.puts(" X axis Y axis Z axis\n\r");
jplager3 0:c29fc80c3ca3 139 dev.puts(" X axis Y axis Z axis\n\r");
jplager3 0:c29fc80c3ca3 140 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
jplager3 0:c29fc80c3ca3 141 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
jplager3 0:c29fc80c3ca3 142 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 0:c29fc80c3ca3 143 dev.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 0:c29fc80c3ca3 144 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jplager3 0:c29fc80c3ca3 145 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jplager3 0:c29fc80c3ca3 146 BTmutex.unlock();
jplager3 0:c29fc80c3ca3 147 myled = 1;
jplager3 0:c29fc80c3ca3 148 wait(0.5);
jplager3 0:c29fc80c3ca3 149 myled = 0;
jplager3 0:c29fc80c3ca3 150 wait(0.5);
jplager3 0:c29fc80c3ca3 151 }
jplager3 0:c29fc80c3ca3 152 }
jplager3 0:c29fc80c3ca3 153
jplager3 0:c29fc80c3ca3 154 void defaultDrive(){ //default behavior for robot
jplager3 0:c29fc80c3ca3 155 //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
jplager3 0:c29fc80c3ca3 156 forward(0.2);
jplager3 0:c29fc80c3ca3 157 if(IR1 > 0.85) { // this is threshold for collision
jplager3 0:c29fc80c3ca3 158 stop();
jplager3 0:c29fc80c3ca3 159 Thread::wait(250);
jplager3 0:c29fc80c3ca3 160 // check if path to right is clear
jplager3 0:c29fc80c3ca3 161 if(IR2 < .4){
jplager3 0:c29fc80c3ca3 162 turnRight(0.3);
jplager3 0:c29fc80c3ca3 163 while(IR1>0.4){}; //turn until path in front is clear
jplager3 0:c29fc80c3ca3 164 stop();
jplager3 0:c29fc80c3ca3 165 }
jplager3 0:c29fc80c3ca3 166 else {
jplager3 0:c29fc80c3ca3 167 turnLeft(0.3);
jplager3 0:c29fc80c3ca3 168 while(IR1>0.4){}; //execute turn until front IR says path is clear
jplager3 0:c29fc80c3ca3 169 // consider placing Thread::wait(??) within loop to account for IR polling?
jplager3 0:c29fc80c3ca3 170 stop();
jplager3 0:c29fc80c3ca3 171 //Thread::wait(250);
jplager3 0:c29fc80c3ca3 172 }
jplager3 0:c29fc80c3ca3 173 Thread::wait(250);
jplager3 0:c29fc80c3ca3 174 forward(0.2);
jplager3 0:c29fc80c3ca3 175
jplager3 0:c29fc80c3ca3 176 /*PICK UP FROM HERE
jplager3 0:c29fc80c3ca3 177 Implement logic to control two scenarios:
jplager3 0:c29fc80c3ca3 178 1) Roomba has detected obstacle in front, but Right is clear. Has turned right and needs to continue driving
jplager3 0:c29fc80c3ca3 179 2) Roomba has detected obstacle, Right is blocked. Turn left & drive until Right is clear. Turn back to right (orig. Fwd heading) and continue.
jplager3 0:c29fc80c3ca3 180 2a) Consider more complex routing to circle around obstacle
jplager3 0:c29fc80c3ca3 181 */
jplager3 0:c29fc80c3ca3 182
jplager3 0:c29fc80c3ca3 183
jplager3 0:c29fc80c3ca3 184 /*
jplager3 0:c29fc80c3ca3 185 //while(IR2>0.5 && IR1<0.8){}; // drive until roomba has passed object.
jplager3 0:c29fc80c3ca3 186 while(IR1<0.8){};
jplager3 0:c29fc80c3ca3 187 stop();
jplager3 0:c29fc80c3ca3 188 Thread::wait(250);
jplager3 0:c29fc80c3ca3 189 //check that path in front is clear
jplager3 0:c29fc80c3ca3 190 if(IR1>0.8){ // if not clear, turn left again until front is clear
jplager3 0:c29fc80c3ca3 191 turnLeft(0.3);
jplager3 0:c29fc80c3ca3 192 while(IR1>0.4){}
jplager3 0:c29fc80c3ca3 193 stop();
jplager3 0:c29fc80c3ca3 194 Thread::wait(250);
jplager3 0:c29fc80c3ca3 195
jplager3 0:c29fc80c3ca3 196 }
jplager3 0:c29fc80c3ca3 197 else {
jplager3 0:c29fc80c3ca3 198
jplager3 0:c29fc80c3ca3 199 }
jplager3 0:c29fc80c3ca3 200
jplager3 0:c29fc80c3ca3 201
jplager3 0:c29fc80c3ca3 202 Thread::wait(200);
jplager3 0:c29fc80c3ca3 203
jplager3 0:c29fc80c3ca3 204 while(IR2>0.85 ) forward(0.3); // drive until
jplager3 0:c29fc80c3ca3 205 */
jplager3 0:c29fc80c3ca3 206
jplager3 0:c29fc80c3ca3 207
jplager3 0:c29fc80c3ca3 208
jplager3 0:c29fc80c3ca3 209 }
jplager3 0:c29fc80c3ca3 210
jplager3 0:c29fc80c3ca3 211 }
jplager3 0:c29fc80c3ca3 212
jplager3 0:c29fc80c3ca3 213 int main()
jplager3 0:c29fc80c3ca3 214 {
jplager3 0:c29fc80c3ca3 215 thread1.start(IMU); // start the IMU thread
jplager3 0:c29fc80c3ca3 216 //thread2.start(defaultDrive);
jplager3 0:c29fc80c3ca3 217
jplager3 0:c29fc80c3ca3 218 forward(0.3);
jplager3 0:c29fc80c3ca3 219 led4=1;
jplager3 0:c29fc80c3ca3 220 while(1){
jplager3 0:c29fc80c3ca3 221
jplager3 0:c29fc80c3ca3 222 }
jplager3 0:c29fc80c3ca3 223
jplager3 0:c29fc80c3ca3 224 }
jplager3 0:c29fc80c3ca3 225