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@4:08a4ae8db71f, 2017-05-02 (annotated)

Committer:
CRaslawski
Date:
Tue May 02 05:06:12 2017 +0000
Revision:
4:08a4ae8db71f
Parent:
3:a739432e805b 
collab test

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