Craig Raslawski / Mbed 2 deprecated IMURoomba3

Roomba Final Project with working RTOS

Dependencies:   4DGL-uLCD-SE SDFileSystem ShiftBrite mbed-rtos mbed wave_player

Fork of RTOS_threadingWorking by Craig Raslawski

main.cpp@15:7cafe0b078ea, 2017-05-02 (annotated)

Committer:
CRaslawski
Date:
Tue May 02 06:11:03 2017 +0000
Revision:
15:7cafe0b078ea
Parent:
14:088c1b04b4c1 
Working RTOS 5 for Roomba!!

Who changed what in which revision?

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