James Plager / Mbed 2 deprecated IMURoomba_FinalProj

Ece4180 final proj. not complete and doesn't compile bc weird header file error lololol

Dependencies:   LSM9DS1_Library_cal Motor mbed-rtos mbed wave_player

Fork of IMURoomba by Craig Raslawski

main.cpp@3:a739432e805b, 2017-05-01 (annotated)

Committer:
jplager3
Date:
Mon May 01 21:59:37 2017 +0000
Revision:
3:a739432e805b
Parent:
2:516dd8a72972
Child:
4:08a4ae8db71f
First upload of ece4180 final proj. Doesn't even compile bc weird error so lol

Who changed what in which revision?

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