main.cpp

00001 /* Megha Sharma, Rishabh Sehgal, Ragheshwar Singh Ranawat ,E.C.E.D NIT Hamirpur
00002    
00003    OBSTACLE AVOIDING ROBOT USING ULTRASONIC SENSOR HCSR04
00004    
00005 In this project LPC 1768 embed board along with NGX Expresso baseboard are successfuly used to make an obstacle avoiding/ 
00006 self navigating robot which makes use of Ultrasonic Sensor HCSR04 for obstacle detection and Ranging. The maximum range of 
00007 Ultrasonic sensor is about 3-4 meters. A program called "ping" sends a serial trigger pulse of 10 nS to the Trigger pin of
00008 HCSR04 and on receiving the trigger HCSR04 beams out an 8 cycle Sonic burst. Then Echo pulse is received by the Ultrasonic sensor 
00009 upto 25ms. On receiving the Echo pulse distance is calculated. The HCSR04 is mounted Axially on a servo motor. Initially the HCSR04
00010 is kept in a straight direction. Motors make bot move in forward direction and continuosly calculating the distance of the 
00011 nearest obstacle in the forward direction. After seeing a distance at less than a "Constraint Value" (i.e. 10 cm here), 
00012 the robot goes in a scan mode. It first moves the Servo Motor to the Right, waits for 2 seconds and stores the value of 
00013 Distance in right direction in "rightscanval" variable.Then it comes back to centre position. Now it moves towards the 
00014 Left direction, waits for 2 seconds and stores the value of Distance in "leftscanval" variable. Then it comes back to the 
00015 centre position. Now it does the comparison of the two variables to determine in which direction it should move. It 
00016 successfully turns towards the direction with the farthest distance of obstacle. 
00017 
00018 Again the Servo motor is in forward direction keeping the sensor straight and the Robot also moves towards Forward direction all
00019 the while calculating the distance of obstacle in forward direction. The loop continues.....
00020 
00021 */
00022 #include "mbed.h"
00023 #include "hcsr04.h"
00024 #include "motordriver.h"
00025 #include "TextLCD.h"
00026 #include "Servo.h"
00027 int ping ();
00028 void scan();
00029 void right (void);  
00030 void stop (void );
00031 void straight (void);
00032 void left (void);
00033 Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
00034 Motor B(p21, p7, p8, 1);
00035 HCSR04  usensor(p13,p14);
00036 TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
00037 PwmOut servo(p23); // Servo pin 21
00038 
00039 
00040 unsigned int dist;
00041 DigitalOut myled(LED1);
00042 DigitalOut myled1(LED2);
00043 DigitalOut myled2(LED3);
00044 int count = 0;
00045 
00046 int main ()
00047     {
00048     
00049          while (1)                // continuos loop to perform the scanning function of HCSR04 .. 
00050                                   //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
00051             {
00052 
00053                 servo.period(0.020); 
00054                 int distance = ping();   // ping function
00055                 straight();        // use the ping() function to see if anything is ahead.
00056    
00057                         
00058                   if (distance < 10) // the condition for stopping the Obstacle Avoider... 
00059                                     // ...whenever the defined proximity to obstacle is reached.
00060                     {   
00061                         stop();
00062                         scan();
00063                         
00064                         
00065                         /*switch (a) // switch case for initial testing of the Obstacle detection by sensor and 
00066                                      // Corresponding L.E.D. of the best direction will glow. 
00067     
00068                              {
00069                                case 'r':
00070                                  myled=1;
00071                                  break;
00072                                case 'l':
00073                                  myled1=1;
00074                                  break;
00075                                case 'f':
00076                                 myled2=1; 
00077                                   break;
00078                                   }*/
00079     
00080                      }
00081             }      
00082     }
00083                       
00084                // A functon to perform the continuos pinging function of HCSR04 .. 
00085                //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin  
00086                          
00087         int ping()                  // ultrasonic sensor is started and data collected is displayed on the LCD
00088                                     // ping function returns an int value of distance scanned
00089                     {
00090                         usensor.start();
00091                         wait_ms(200); 
00092                         dist=usensor.get_dist_cm();
00093                         lcd.cls();
00094                         lcd.locate(0,0);
00095                         if (dist <50)
00096                         lcd.printf("cm:%ld",dist );
00097                         else
00098                         lcd.printf("no object");
00099                         return (dist);
00100                          
00101                      }
00102         
00103         
00104          void scan () 
00105             {
00106                          
00107                 int leftscanval, rightscanval;       // variables for storing distance scan values...
00108                                                      //... of nearest obstacle in the left and right direction
00109                                                      
00110                                                      
00111           // Scanning RIGHT side; turning the servo attached to HCSR04 right to check for obstacle distance
00112                         
00113                         // 0.0016 corresponds to the neutral position of servo motor 
00114                         // 0.0027 corresponds to the rightmost position of the servo motor 
00115                         // 0.0009 corresponds to the leftmost position of the servomotor
00116                     
00117                     for(float offset=0.0016; offset<=0.0027; offset+=0.0001)      // turning RIGHT
00118                         
00119                         {
00120                             servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
00121                             wait(0.10);
00122                         }
00123                             wait (2);                 // stopping at RIGHTMOST position for 2 seconds
00124                             rightscanval = ping();    // putting the distance of right direction's obstacle in ''rightscanval variable'' 
00125                     
00126                     
00127                     
00128                     for(float offset=0.0027; offset>=0.0016; offset+=-0.0001)     // turning back to the CENTER position
00129                         
00130                         {
00131                             servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
00132                             wait(0.10);
00133                         }
00134                             wait(2);                  // stopping at CENTERMOST position for 2 seconds
00135             
00136             
00137           // Scanning LEFT side; turning the servo attached to HCSR04 right to check for obstacle distance
00138           
00139           
00140                     for(float offset=0.0016; offset>=0.0009; offset-=0.0001)    // Turning towards LEFT
00141                         
00142                         {
00143                             servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
00144                             wait(0.10);
00145                         }
00146                             wait(2);                   // stopping at LEFTMOST position for 2 seconds                       
00147                             leftscanval = ping();      // putting the distance of right direction's obstacle in ''leftscanval variable''
00148                   
00149                     
00150                     for(float offset=0.0009; offset<=0.0016; offset+=0.0001) 
00151                     
00152                         {
00153                             servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
00154                             wait(0.10);
00155                         }
00156                   /*center scan servo (for changing the default position of Servo to the Centre Position ) This method didnt work 
00157                     though. There is a library of Servo 
00158                     */
00159                   //servo.pulsewidth(0.00017);
00160                  
00161              
00162  /* Comparisons to choose the most appropiate direction for the turning of Robot. Based on the distances stored in variables "leftscanvalue" and "rightscanvalue"  */
00163              
00164              if (leftscanval>rightscanval )
00165                   
00166                     {
00167                         left();
00168                     }
00169                     
00170              else if (rightscanval>leftscanval )
00171                     
00172                     {
00173                        right();
00174                     }
00175                     
00176              else
00177                     {
00178                         left();
00179                     }
00180                     
00181                   //return choice;
00182                          }
00183                          
00184          void stop ()           // function for stopping the DC motors
00185              
00186              {
00187                 A.speed(0); 
00188                 B.speed(0);
00189              }
00190          
00191          
00192          void left()            // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so... 
00193                                 // ..that the robot turns to the LEFT
00194             
00195             {
00196                 for (float s= -1.0; s < 0.0 ; s += 0.01) 
00197                     {
00198                         A.speed(-1); 
00199                         B.speed(-1); 
00200                         wait(0.02);
00201                     }                
00202             }
00203             
00204             
00205         void right ()           // function for moving the RIGHT motor in REVERSE direction and LEFT motor in FORWARD direction so... 
00206                                 // ..that the robot turns to the LEFT
00207             
00208             {
00209                 for (float s= -1.0; s < 0.0 ; s += 0.01) 
00210                     {
00211                         A.speed(1); 
00212                         B.speed(1); 
00213                         wait(0.02);
00214                     }                
00215             }
00216 
00217 
00218         void straight ()        // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so... 
00219                                 // ..that the robot turns to the LEFT
00220 
00221             {            
00222                 //for (float s= -1.0; s < -0.7 ; s += 0.01) 
00223                 {
00224                     A.speed(-1); 
00225                     B.speed(1); 
00226                     wait(0.02);
00227                 }
00228                 
00229             }