main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "Servo.h"
00004 #include "string.h"
00005 #define HIGH 1
00006 #define LOW 0
00007 
00008 void forward();
00009 void backward();
00010 void right();
00011 void left();
00012 void stop();
00013 void loop();
00014 
00015 void blueInterrupt();
00016 void timerInterrupt();
00017 void bluecheck();
00018 
00019 int distance;                 //Variable to store distance from an object
00020 int checkRight;
00021 int checkLeft;
00022 int function=1;               //Variable to store function of robot: '1' running or '0' stoped. By 
00023 int pos=90; 
00024 int flagBlue=0;
00025 int flagTimer=0;
00026 
00027 Ticker timer;
00028 
00029 
00030 int  c;
00031 
00032 
00033 Serial pc(USBTX,USBRX);
00034 Serial blue(PTC15,PTC14);
00035 PwmOut servo(D6);    
00036 
00037 char send[512],rcv[1000];//Variables
00038 
00039 int main()
00040 {
00041      
00042      blue.baud(9600);
00043      
00044  while(1)
00045  {        
00046      if(!blue.readable())
00047      {
00048             !blue.gets(rcv,1000);
00049             if(strstr(rcv,"forward")!=NULL)
00050             {
00051                 forward();
00052                 }
00053             else if(strstr(rcv,"backward")!=NULL)
00054             {
00055                 backward();
00056                 }
00057              else if(strstr(rcv,"right")!=NULL)
00058             {
00059                 right();
00060                 }
00061                 else if(strstr(rcv,"left")!=NULL)
00062                 {
00063                     left();     
00064                     }                 
00065               }                          
00066          else                
00067                 {                                        
00068                     stop();                    
00069                     }   
00070 }
00071 }   
00072     
00073 void forward(){
00074 
00075     
00076 
00077     pc.printf("fwd");
00078 
00079     DigitalOut(D3,1);
00080 
00081     DigitalOut(D5, 0);
00082 
00083     DigitalOut(D10, 1);
00084 
00085     DigitalOut(D11, 0); 
00086 
00087 }
00088 
00089 void backward(){
00090 
00091     DigitalOut(D3,0);
00092 
00093     DigitalOut(D5, 1);
00094 
00095     DigitalOut(D10, 0);
00096 
00097     DigitalOut(D11, 1); 
00098 
00099 }
00100 
00101 
00102 void right()
00103 
00104 
00105 {
00106   
00107 
00108     DigitalOut(D3,0);
00109 
00110     DigitalOut(D5, 1);
00111 
00112     DigitalOut(D10, 1);
00113 
00114     DigitalOut(D11, 0); 
00115 
00116 
00117     }
00118 
00119    
00120     void left()
00121 
00122 
00123 {
00124 
00125     DigitalOut(D3,1);
00126 
00127     DigitalOut(D5, 0);
00128 
00129     DigitalOut(D10, 0);
00130 
00131     DigitalOut(D11, 1); 
00132 
00133     
00134     }
00135 
00136 
00137 
00138   void stop()
00139 
00140 
00141 {
00142 
00143 
00144     DigitalOut(D3,0);
00145 
00146     DigitalOut(D5, 0);
00147 
00148     DigitalOut(D10,0);
00149 
00150     DigitalOut(D11, 0); 
00151 
00152 
00153     }
00154     
00155   /*  
00156     void loop()
00157     
00158     {
00159         
00160            servo.period(0.020);
00161             usensor.start();
00162         wait_ms(500); 
00163         distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
00164         pc.printf("Dist: %d ",distance);
00165         
00166         //Check for objects...
00167         if (distance > 50){
00168             pc.printf(" No Object \n");
00169             forward(); //All clear, move forward!
00170             //noTone(buzzer);
00171            // digitalOut LED(0);
00172         }
00173         else if (distance <=50){
00174             pc.printf("Halt - Object on the Way \n");
00175             stop();
00176             
00177 
00178                for(float offset=0.0016; offset<=0.0020; offset+=0.0001) // turning RIGHT
00179 
00180 {
00181     
00182       pc.printf("Turning Right"); 
00183 
00184 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00185 
00186 wait_ms(0.10);
00187 
00188 }
00189 
00190 wait_ms(200); // stopping at RIGHTMOST position for 2 seconds
00191 
00192     
00193         usensor.start();
00194 
00195         wait_ms(500); 
00196 
00197             checkRight = usensor.get_dist_cm();      //Take distance from the left side
00198 
00199             pc.printf("DistL: %d ",checkRight); 
00200             
00201       
00202             for (float offset=0.0020; offset>=0.0016;) // turning back to theCENTER position
00203 {
00204  pc.printf("Turning Center"); 
00205 
00206 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00207 offset=offset-0.0001;
00208 
00209 wait_ms(0.10);
00210 
00211 }
00212 
00213 wait_ms(200);
00214 
00215             
00216 for (float offset=0.0016; offset>=0.0012; offset=offset-0.0001) // Turning towards LEFT
00217 
00218 {
00219 
00220  pc.printf("Turning Left"); 
00221 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00222 
00223 wait_ms(0.10);
00224 
00225 }
00226 
00227 wait_ms(200); // stopping at LEFTMOST position for 2
00228             
00229 
00230         usensor.start();
00231 
00232         wait_ms(500); 
00233 
00234             checkLeft= usensor.get_dist_cm();
00235 
00236             pc.printf("DistR: %d ",checkLeft); 
00237 
00238             
00239 for(float offset=0.0012; offset<=0.0016; offset=offset+0.0001)
00240 
00241 {
00242     pc.printf("Turning Center"); 
00243 
00244 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00245 
00246 wait_ms(0.10);
00247 
00248 }
00249             
00250 
00251             //Finally, take the right decision, turn left or right?
00252 
00253             if (checkLeft < checkRight){
00254                 
00255                  backward();
00256                    wait_ms(1000);
00257 
00258 
00259                 left();
00260 pc.printf("goingleft");
00261                 wait_ms(400);
00262                 
00263                  // wait_ms, change value if necessary to make robot turn.            
00264               forward();
00265               pc.printf("now going straight");
00266                 }
00267 
00268             else if (checkLeft > checkRight){
00269                 
00270                 backward();
00271                    wait_ms(1000);
00272 
00273                 right();
00274             
00275             
00276 
00277                 wait_ms(400);
00278                 forward();
00279                  // wait_ms, change value if necessary to make robot turn.
00280 
00281             }
00282 
00283             else if (checkLeft <=10 && checkRight <=10){
00284 
00285                 backward();
00286                  wait_ms(1000); //The road is closed... go back and then left ;)
00287 
00288                 left();
00289                  wait_ms(400);
00290                 forward();
00291 
00292             }
00293 
00294         }
00295 */
00296