A final design project for embedded system class

Dependencies:   HMC6352 mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 
00002 //#include "HMC6352.h" //compass
00003 #include "mbed.h"
00004 //HMC6352 compass(p28, p27);
00005 Serial pc(USBTX, USBRX);
00006 AnalogIn Rdist(p19); // right sonar
00007 AnalogIn Ldist(p20);// left sonar
00008 AnalogIn Fdist(p18);// front sonar
00009 AnalogIn range(p17); // right range finder
00010 Serial device(p9, p10);  // tx, rx
00011 DigitalOut leftlight(LED1);
00012 DigitalOut rightlight(LED3);
00013 DigitalOut straightlight(LED2);
00014 
00015 // Definitions of iRobot Create OpenInterface Command Numbers
00016 // See the Create OpenInterface manual for a complete list
00017 
00018 
00019 //                 Create Command              // Arguments
00020 const char         Start = 128;
00021 const char         SafeMode = 131;
00022 const char         FullMode = 132;
00023 const char         Drive = 137;                // 4:   [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
00024 const char         DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
00025 const char         Demo = 136;                 // 2:    Run Demo x
00026 const char         Sensors = 142;              // 1:    Sensor Packet ID
00027 const char         CoverandDock = 143;         // 1:    Return to Charger
00028 const char         SensorStream = 148;               // x+1: [# of packets requested] IDs of requested packets to stream
00029 const char         QueryList = 149;            // x+1: [# of packets requested] IDs of requested packets to stream
00030 const char         StreamPause = 150;          // 1:    0 = stop stream, 1 = start stream
00031 const char         PlaySong = 141;
00032 const char         Song = 140;
00033 /* iRobot Create Sensor IDs */
00034 const char         BumpsandDrops = 7;
00035 const char         Distance = 19;
00036 const char         Angle = 20;
00037 
00038 static int speed_left =  200;
00039 static int speed_right = 200;
00040 static int state=0;
00041 //static int arr[10];//0=forwatd, 1=right,2=left,3=finish;
00042 void start();
00043 void forward();
00044 void reverse();
00045 void left();
00046 void right();
00047 void stop();
00048 void playsong();
00049 void charger();
00050 float curveR=0.02;
00051 float curveL=0.02;
00052 float Hsouth=98;
00053 float Hnorth=68;
00054 float Hwest=80;
00055 float Heast=77;
00056 // Demo to move around using basic commands
00057 int main()
00058 {
00059     int arr[5]= {0,1,0,1,40};//0=forwatd, 1=right,2=left,3=finish;
00060     float time=0;
00061 // wait for Create to power up to accept serial commands
00062     wait(3);
00063     //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00064 // set baud rate for Create factory default
00065     device.baud(57600);
00066 // Start command mode and select sensor data to send back
00067     start();
00068     wait(.5);
00069 // Move around with motor commands
00070     while(1) {
00071 
00072         if((Rdist>0.49 || Ldist>0.49)&&(arr[state]<3)) {
00073             leftlight=1;
00074             rightlight=1;
00075             straightlight=1;
00076             if(float(Fdist)<0.12 ) {
00077                 stop();
00078                 wait(1);
00079                 if(float(Fdist)<0.13 ) {
00080                     state++;
00081                     if(arr[state]==1) { //right turn
00082                         speed_right=200;
00083                         speed_left=200;
00084                         right();
00085                         wait(1.0);
00086                     } else if(arr[state]==2) {
00087                         speed_right=200;
00088                         speed_left=200;
00089                         left();
00090                         wait(1.0);
00091                     }//left
00092                     stop();
00093                     wait(0.2);
00094                     forward();
00095                     wait(6);
00096                     state++;
00097                 }
00098             }
00099 
00100             // else {
00101             //   speed_right=200;
00102             // speed_left=200;
00103             //}
00104         } else if((float(Ldist)-float(Rdist))>curveL ) {
00105             speed_right++;
00106             leftlight=1;
00107             rightlight=0;
00108             straightlight=0;
00109             if(speed_right>206) {
00110                 speed_right=206;
00111             }
00112         } else if ( (float(Rdist)-float(Ldist))>curveR ) {
00113             speed_left++;
00114             leftlight=0;
00115             rightlight=1;
00116             straightlight=0;
00117             if(speed_left>206) {
00118                 speed_left=206;
00119             }
00120         }
00121 
00122         else if (((float(Ldist)-float(Rdist))<curveL) && ( (float(Rdist)-float(Ldist))<curveR )) {
00123             speed_left=200;
00124             speed_right=200;
00125             leftlight=0;
00126             rightlight=0;
00127             straightlight=1;
00128         }
00129         if(arr[state]>2) {
00130             time=time+0.1;
00131             if(time>arr[state]) {
00132                 break;//while
00133             }
00134         }
00135 
00136         //pc.printf("Left: %f    ", float(Ldist));
00137         pc.printf("Right: %f   ", float(Rdist));
00138         pc.printf("range: %f   ", float(range));
00139         //pc.printf("front: %f   ", float(Fdist));
00140         //pc.printf("Diff: %f\n", (float(Rdist)-float(Ldist)));
00141         forward();
00142         wait(0.1);
00143     }
00144 // Play a song
00145     stop();
00146     playsong();
00147     wait(10);
00148 // Search for battery charger IR beacon
00149     // charger();
00150 }
00151 
00152 
00153 // Start  - send start and safe mode, start streaming sensor data
00154 void start()
00155 {
00156     // device.printf("%c%c", Start, SafeMode);
00157     device.putc(Start);
00158     device.putc(SafeMode);
00159     wait(.5);
00160     //  device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
00161     device.putc(SensorStream);
00162     device.putc(1);
00163     device.putc(BumpsandDrops);
00164     wait(.5);
00165 }
00166 // Stop  - turn off drive motors
00167 void stop()
00168 {
00169     device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
00170 }
00171 // Forward  - turn on drive motors
00172 void forward()
00173 {
00174     device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),
00175                   char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
00176 
00177 }
00178 // Reverse - reverse drive motors
00179 void reverse()
00180 {
00181     device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),
00182                   char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
00183 
00184 }
00185 // Left - drive motors set to rotate to left
00186 void left()
00187 {
00188     device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),
00189                   char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
00190 }
00191 // Right - drive motors set to rotate to right
00192 void right()
00193 {
00194     device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),
00195                   char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
00196 
00197 }
00198 // Charger - search and return to charger using IR beacons (if found)
00199 void charger()
00200 {
00201     device.printf("%c%c", Demo, char(1));
00202 }
00203 // Play Song  - define and play a song
00204 void playsong()   // Send out notes & duration to define song and then play song
00205 {
00206 
00207     device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
00208                   Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
00209                   char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
00210                   char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
00211                   char(24), char(86), char(12), char(87), char(48));
00212 
00213     wait(.2);
00214     device.printf("%c%c", PlaySong, char(0));
00215 }