main.cpp

00001 #include "mbed.h"
00002 #include "wave_player.h"
00003 #include "SDFileSystem.h"
00004 #include "Servo.h"
00005 #include "ultrasonic.h"
00006 #include "uLCD_4DGL.h"
00007 #include "LSM9DS1.h"
00008 
00009 volatile bool global_running = true; //1 = display distance, 0 = avoid collision
00010 float range = 0.0005; // for servo/motors calibration
00011 float position = 0.5; // for servo/motors calibration
00012 unsigned int frame_limiter = 0; // Limits the framerate of the lcd
00013 
00014 Serial pc(USBTX, USBRX);
00015 uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin;
00016 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00017 
00018 
00019 Servo dcmotorBottom(p21); // pwm
00020 Servo servoBottom(p23); // pwm
00021 Servo dcmotorBack(p22); // pwm
00022 
00023 // Setup to play wav file from SD Card
00024 AnalogOut DACout(p18);
00025 wave_player waver(&DACout);
00026 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
00027 
00028 // Setup for bluetooth
00029 Serial blue(p28,p27);
00030 DigitalOut myled1(LED1);
00031 DigitalOut myled2(LED2);
00032 DigitalOut myled3(LED3);
00033 DigitalOut myled4(LED4);
00034 DigitalOut exled1(p19);
00035 DigitalOut exled2(p26);
00036 
00037 
00038 // Setup for IMU
00039 #define PI 3.14159
00040 // Earth's magnetic field varies by location. Add or subtract
00041 // a declination to get a more accurate heading. Calculate
00042 // your's here:
00043 // http://www.ngdc.noaa.gov/geomag-web/#declination
00044 #define DECLINATION -4.94*PI/180 // Declination (radians) in Atlanta,GA.
00045 // Calculate heading.
00046 // Heading calculations taken from this app note:
00047 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00048 float calculateHeading(float mx, float my)
00049 {
00050 // touchy trig stuff to use arctan to get compass heading
00051     mx = -mx;
00052     float heading;
00053     if (my == 0.0)
00054         heading = (mx < 0.0) ? PI : 0.0;
00055     else
00056         heading = atan2(mx, my);
00057     //pc.printf("heading atan=%f \n\r",heading);
00058     heading -= DECLINATION; //correct for geo location
00059     if(heading>PI) heading = heading - 2*PI;
00060     else if(heading<-PI) heading = 2*PI + heading;
00061     else if(heading<0.0) heading = 2*PI + heading;
00062     return heading;
00063 }
00064 
00065 // ESC/ Brushless DC Motor setup
00066 void col_ESC() {
00067     dcmotorBottom = 0.0;
00068     dcmotorBack = 0.0;
00069     wait(0.5); //ESC detects signal
00070     //Required ESC Calibration/Arming sequence  
00071     //sends longest and shortest PWM pulse to learn and arm at power on
00072     dcmotorBottom = 1.0; //send longest PWM
00073     dcmotorBack = 1.0; //send longest PWM
00074     wait(8);
00075     dcmotorBottom = 0.0; //send shortest PWM
00076     dcmotorBack = 0.0; //send shortest PWM
00077     wait(8);
00078 }
00079 
00080 
00081 void dist(int distance) 
00082 {   
00083     // Set global running flag based on distance to sonar
00084     if(distance < 100){
00085         global_running = false;
00086     }
00087     else{
00088         if (global_running == false){
00089             global_running = true;
00090             // Wait 2 seconds before starting again
00091             //wait(2);
00092         }
00093     }
00094     // If there is an obstruction, print the distance until it is clear
00095     if(!global_running){
00096         // Only update once every 4 loops
00097         if (frame_limiter % 2 == 0){
00098             uLCD.cls();
00099             uLCD.printf("Distance:\r\n%dmm\r\n", distance); //print to uLCD
00100         }
00101         frame_limiter++;
00102     }
00103 
00104     
00105 }
00106 ultrasonic mu(p15, p16, .1, 1, &dist);
00107 //have updates every .1 seconds and a timeout after 1
00108 //second, and call dist when the distance changes
00109 
00110 void honk(){
00111     FILE *wave_file;
00112     wave_file=fopen("/sd/honk.wav","r"); 
00113     waver.play(wave_file); 
00114     fclose(wave_file);  
00115     myled1=!myled1;
00116     myled2=!myled2;
00117     myled3=!myled3;
00118     myled4=!myled4;
00119 }
00120 
00121 
00122 
00123 int main()
00124 {   
00125     uLCD.reset();
00126     //calibrate motors
00127     servoBottom.calibrate(range, position);
00128     col_ESC();
00129     
00130     Ticker honk_int;
00131     
00132     char bnum = 0;
00133     char bhit = 0;
00134     float speed = 0.3;
00135     uLCD.baudrate(BAUD_3000000); //jack up baud rate to max for fast display
00136     wait(0.5);
00137 
00138     IMU.begin();
00139     if (!IMU.begin()) {
00140         // red circle of death
00141         uLCD.circle(63,63, 51, RED);
00142     }
00143     IMU.calibrate(1);
00144     uLCD.cls();
00145     uLCD.printf("Calibrating:\r\nSpin Me!\r\n"); //print to uLCD
00146     IMU.calibrateMag(0);
00147     uLCD.cls();
00148     
00149     int x=0;
00150     int y=50;
00151     mu.startUpdates();//start measuring the distance
00152     bool start_motor = true;
00153     bool moving = false;
00154     
00155     
00156     while(1) 
00157     {   
00158         // Check for collision
00159         mu.checkDistance();
00160         // If the motor is stopped and there are no collisions, start the motor
00161         if (global_running && start_motor){
00162             for (float p=0.0; p<=0.9; p += 0.1) { //Throttle up slowly to full throttle
00163                 dcmotorBottom = p;
00164                 wait(0.2);
00165             }
00166             // Draw the circle for the compass on the LCD
00167             uLCD.cls();
00168             uLCD.circle(63,63, 51, WHITE);
00169             start_motor = false;
00170 
00171         }
00172         if (global_running){
00173             // Update the compass reading
00174             // Only update once every 150000 loops
00175             if (frame_limiter % 100000 == 0){
00176                 if(IMU.magAvailable(X_AXIS)){
00177                     IMU.readMag();
00178                     int prev_x = x;
00179                     int prev_y = y;
00180                     float heading = calculateHeading(IMU.mx, IMU.my);
00181                     x = 63+50*sin(heading+PI)+ 0.5;
00182                     y = 63+50*cos(heading+PI)+ 0.5;
00183                     uLCD.line(63, 63, prev_x, prev_y, BLACK);
00184                     uLCD.line(63, 63, x, y, RED);
00185                     uLCD.printf("%.2f\r", heading*180/PI);
00186                 }
00187             }
00188             frame_limiter++;
00189             // Read bluetooth controller input
00190             if (blue.readable()){
00191                 if (blue.getc() == '!') {
00192                     if (blue.getc() == 'B') { //button data packet
00193                         bnum = blue.getc(); //button number
00194                         bhit = blue.getc(); //1=hit, 0=release
00195                         if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
00196                             switch (bnum) {
00197                                 case '1': //number button 1: Increase max speed
00198                                     if (bhit=='1') {
00199                                         //add hit code here
00200                                         if(speed < 0.3){
00201                                             speed = speed + 0.06;
00202                                         }
00203                                     } else {
00204                                         // When button is released, set rear motor to max speed
00205                                         // if it is currently running
00206                                         if(moving) dcmotorBack = speed;
00207                                     }
00208                                     break;
00209                                 case '2': //number button 2: Decrease max speed
00210                                     if (bhit=='1') {
00211                                         //add hit code here
00212                                         if(speed > 0.06){
00213                                             speed = speed - 0.06;
00214                                         }
00215                                     } else {
00216                                         // When button is released, set rear motor to max speed
00217                                         // if it is currently running
00218                                         if(moving) dcmotorBack = speed;
00219                                     }
00220                                     break;
00221                                 case '3': //number button 3: light the external leds
00222                                     if (bhit=='1') {
00223                                         exled1 = !exled1;
00224                                         exled2 = !exled2;
00225                                     } else {
00226                                         //add release code here
00227                                     }
00228                                     break;
00229                                 case '4': //number button 4: play honking sound
00230                                     if (bhit=='1') {
00231                                         honk();                       
00232                                     } else {
00233                                         //add release code here
00234                                     }
00235                                     break;
00236                                 case '5': //button 5 up arrow: turn on bottom servo and back motor
00237                                     if (bhit=='1') {
00238                                         //add hit code here  
00239                                         for (float p=0.0; p<=speed; p += 0.06) { //Throttle up slowly to full throttle
00240                                             dcmotorBack = p;
00241                                             wait(0.2);
00242                                         }
00243                                         moving = true;
00244                                                                
00245                                     } else {
00246                                         //add release code here
00247                                         dcmotorBack = 0.0;
00248                                         moving = false;
00249                                     }
00250                                     break;
00251                                 case '6': //button 6 down arrow: switch it off (turn off back motor)
00252                                     if (bhit=='1') {
00253                                         
00254                                     } else {
00255                                         //add release code here
00256                                     }
00257                                     break;
00258                                 case '7': //button 7 left arrow: turns servo leftwards
00259                                     if (bhit=='1') {
00260                                        servoBottom = 1.0;
00261                                     } else {
00262                                         //add release code here
00263                                         servoBottom = 0.5;
00264                                     }
00265                                     break;
00266                                 case '8': //button 8 right arrow: turns servo rightwards
00267                                     if (bhit=='1') {
00268                                         servoBottom = 0.0;
00269                                     } else {
00270                                         //add release code here
00271                                         servoBottom = 0.5;
00272                                     }
00273                                     break;
00274                                 default:
00275                                     break;
00276                             }
00277                         }
00278                     }
00279                 }// End of bluetooth controller loop
00280             }
00281         }
00282         else
00283         {
00284             // Collision detection: Stop all motors and honk until obstruction is cleared
00285             dcmotorBack = 0.0;
00286             dcmotorBottom = 0.0;
00287             servoBottom = 0.5;
00288             start_motor = true;
00289             moving = false;
00290             unsigned int honk_limiter = 0;
00291             while(!global_running){
00292                 // Check if the obstruction is clear
00293                 mu.checkDistance();
00294                 // Honk every once in a while
00295                 if(honk_limiter % 6000000 == 0){
00296                     honk();
00297                 }
00298                 honk_limiter++;
00299                 
00300             }
00301              
00302         }
00303         
00304     }
00305 }