main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "Servo.h"
00004 #define HIGH 1
00005 #define LOW 0
00006 void forward();
00007 void backward();
00008 void right();
00009 void left();
00010 void stop();
00011 void loop();
00012 
00013 int distance;                 //Variable to store distance from an object
00014 int checkRight;
00015 int checkLeft;
00016 int function=1;               //Variable to store function of robot: '1' running or '0' stoped. By 
00017 int pos=90; 
00018 
00019 
00020 Serial pc(USBTX,USBRX);
00021 PwmOut servo(D6);    
00022 
00023 
00024 
00025 HCSR04  usensor(A4,A5);
00026 
00027 //Variables
00028 
00029 int main()
00030 {
00031    
00032  while(1)
00033  {  
00034 loop();
00035 }
00036    
00037 }
00038         
00039 
00040 
00041 void forward(){
00042     
00043     pc.printf("fwd");
00044     DigitalOut(D13,0);
00045     DigitalOut(D12, 1);
00046     DigitalOut(D9, 1);
00047     DigitalOut(D8, 0); 
00048 }
00049 
00050 
00051 
00052 void backward(){
00053     DigitalOut(D13,1);
00054     DigitalOut(D12, 0);
00055     DigitalOut(D9, 0);
00056     DigitalOut(D8, 1); 
00057 }
00058 
00059 void right()
00060 
00061 {
00062     
00063     DigitalOut(D13,0);
00064     DigitalOut(D12, 1);
00065     DigitalOut(D9, 0);
00066     DigitalOut(D8, 1); 
00067     
00068     
00069     }
00070     
00071     
00072     
00073     void left()
00074 
00075 {
00076     
00077     DigitalOut(D13,1);
00078     DigitalOut(D12, 0);
00079     DigitalOut(D9, 1);
00080     DigitalOut(D8, 0); 
00081     
00082     
00083     }
00084 
00085 
00086   void stop()
00087 
00088 {
00089     
00090     DigitalOut(D13,0);
00091     DigitalOut(D12, 0);
00092     DigitalOut(D9, 0);
00093     DigitalOut(D8, 0); 
00094     
00095     
00096     }
00097     
00098     
00099     void loop()
00100     
00101     {
00102         
00103            servo.period(0.020);
00104             usensor.start();
00105         wait_ms(500); 
00106         distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
00107         pc.printf("Dist: %d ",distance);
00108         //Check for objects...
00109         if (distance > 10){
00110             pc.printf(" No Object \n");
00111             forward(); //All clear, move forward!
00112             //noTone(buzzer);
00113            // digitalOut LED(0);
00114         }
00115         else if (distance <=10){
00116             pc.printf("Halt - Object on the Way \n");
00117             stop();
00118             
00119 
00120                for(float offset=0.0016; offset<=0.0027; offset+=0.0001) // turning RIGHT
00121 
00122 {
00123     
00124       pc.printf("Turning Right"); 
00125 
00126 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00127 
00128 wait(0.10);
00129 
00130 }
00131 
00132 wait (2); // stopping at RIGHTMOST position for 2 seconds
00133 
00134         
00135    
00136 
00137         usensor.start();
00138 
00139         wait_ms(500); 
00140 
00141             checkRight = usensor.get_dist_cm();      //Take distance from the left side
00142 
00143             pc.printf("DistL: %d ",checkLeft); 
00144             
00145       
00146             for (float offset=0.0027; offset>=0.0016;) // turning back to theCENTER position
00147 {
00148  pc.printf("Turning Center"); 
00149 
00150 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00151 
00152 wait(0.10);
00153 offset=offset-0.0001;
00154 }
00155 
00156 wait(2);
00157 
00158             
00159 for (float offset=0.0016; offset>=0.0009; offset=offset-0.0001) // Turning towards LEFT
00160 
00161 {
00162 
00163  pc.printf("Turning Left"); 
00164 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00165 
00166 wait(0.10);
00167 
00168 }
00169 
00170 wait(2); // stopping at LEFTMOST position for 2
00171             
00172 
00173         usensor.start();
00174 
00175         wait_ms(500); 
00176 
00177             checkLeft= usensor.get_dist_cm();
00178 
00179             pc.printf("DistR: %d ",checkRight); 
00180 
00181             
00182 for(float offset=0.0009; offset<=0.0016; offset=offset+0.0001)
00183 
00184 {
00185     pc.printf("Turning Center"); 
00186 
00187 servo.pulsewidth(offset); // servo position determined by a pulsewidth between
00188 
00189 wait(0.10);
00190 
00191 }
00192             
00193 
00194             //Finally, take the right decision, turn left or right?
00195 
00196             if (checkLeft < checkRight){
00197 
00198                 left();
00199 
00200                 wait_ms(400);
00201                 
00202                  // wait_ms, change value if necessary to make robot turn.            
00203               forward();
00204                 }
00205 
00206             else if (checkLeft > checkRight){
00207 
00208                 right();
00209 
00210                 wait_ms(400);
00211                 forward();
00212                  // wait_ms, change value if necessary to make robot turn.
00213 
00214             }
00215 
00216             else if (checkLeft <=10 && checkRight <=10){
00217 
00218                 backward(); //The road is closed... go back and then left ;)
00219 
00220                 left();
00221                 forward();
00222 
00223             }
00224 
00225         }
00226 
00227     
00228 
00229 }
00230 
00231 
00232     
00233         
00234         
00235         
00236         
00237         
00238        
00239         
00240         
00241 
00242     
00243