monika kakani
this robot uses ultrasonic sensors to detect the obstacles and the servo motor acts as the neck for the robot in checking the obstacles in right and left directions
Dependencies: HCSR04 Servo mbed
Fork of
Obstacle_avoidance
by 
ece nith
Diff: main.cpp
- Revision:
- 1:43568a9d4323
- Parent:
- 0:a87600bebf7b
--- a/main.cpp Sat May 18 04:39:55 2013 +0000
+++ b/main.cpp Fri Mar 24 01:02:04 2017 +0000
@@ -1,229 +1,296 @@
-/* Megha Sharma, Rishabh Sehgal, Ragheshwar Singh Ranawat ,E.C.E.D NIT Hamirpur
-
- OBSTACLE AVOIDING ROBOT USING ULTRASONIC SENSOR HCSR04
-
-In this project LPC 1768 embed board along with NGX Expresso baseboard are successfuly used to make an obstacle avoiding/
-self navigating robot which makes use of Ultrasonic Sensor HCSR04 for obstacle detection and Ranging. The maximum range of
-Ultrasonic sensor is about 3-4 meters. A program called "ping" sends a serial trigger pulse of 10 nS to the Trigger pin of
-HCSR04 and on receiving the trigger HCSR04 beams out an 8 cycle Sonic burst. Then Echo pulse is received by the Ultrasonic sensor
-upto 25ms. On receiving the Echo pulse distance is calculated. The HCSR04 is mounted Axially on a servo motor. Initially the HCSR04
-is kept in a straight direction. Motors make bot move in forward direction and continuosly calculating the distance of the
-nearest obstacle in the forward direction. After seeing a distance at less than a "Constraint Value" (i.e. 10 cm here),
-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
-Distance in right direction in "rightscanval" variable.Then it comes back to centre position. Now it moves towards the
-Left direction, waits for 2 seconds and stores the value of Distance in "leftscanval" variable. Then it comes back to the
-centre position. Now it does the comparison of the two variables to determine in which direction it should move. It
-successfully turns towards the direction with the farthest distance of obstacle.
-
-Again the Servo motor is in forward direction keeping the sensor straight and the Robot also moves towards Forward direction all
-the while calculating the distance of obstacle in forward direction. The loop continues.....
-
-*/
#include "mbed.h"
#include "hcsr04.h"
-#include "motordriver.h"
-#include "TextLCD.h"
#include "Servo.h"
-int ping ();
-void scan();
-void right (void);
-void stop (void );
-void straight (void);
-void left (void);
-Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can brake
-Motor B(p21, p7, p8, 1);
-HCSR04 usensor(p13,p14);
-TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
-PwmOut servo(p23); // Servo pin 21
+#include "string.h"
+#define HIGH 1
+#define LOW 0
+
+void forward();
+void backward();
+void right();
+void left();
+void stop();
+void loop();
+
+void blueInterrupt();
+void timerInterrupt();
+void bluecheck();
+
+int distance; //Variable to store distance from an object
+int checkRight;
+int checkLeft;
+int function=1; //Variable to store function of robot: '1' running or '0' stoped. By
+int pos=90;
+int flagBlue=0;
+int flagTimer=0;
+
+Ticker timer;
+
+
+int c;
+
+
+Serial pc(USBTX,USBRX);
+Serial blue(PTC15,PTC14);
+PwmOut servo(D6);
+
+char send[512],rcv[1000];//Variables
+
+int main()
+{
+
+ blue.baud(9600);
+
+ while(1)
+ {
+ if(!blue.readable())
+ {
+ !blue.gets(rcv,1000);
+ if(strstr(rcv,"forward")!=NULL)
+ {
+ forward();
+ }
+ else if(strstr(rcv,"backward")!=NULL)
+ {
+ backward();
+ }
+ else if(strstr(rcv,"right")!=NULL)
+ {
+ right();
+ }
+ else if(strstr(rcv,"left")!=NULL)
+ {
+ left();
+ }
+ }
+ else
+ {
+ stop();
+ }
+}
+}
+
+void forward(){
+
+
+
+ pc.printf("fwd");
+
+ DigitalOut(D3,1);
+
+ DigitalOut(D5, 0);
+
+ DigitalOut(D10, 1);
+
+ DigitalOut(D11, 0);
+
+}
+
+void backward(){
+
+ DigitalOut(D3,0);
+
+ DigitalOut(D5, 1);
+
+ DigitalOut(D10, 0);
+
+ DigitalOut(D11, 1);
+
+}
+
+
+void right()
+
+
+{
+
+
+ DigitalOut(D3,0);
+
+ DigitalOut(D5, 1);
+
+ DigitalOut(D10, 1);
+
+ DigitalOut(D11, 0);
+
+
+ }
+
+
+ void left()
+
+
+{
+
+ DigitalOut(D3,1);
+
+ DigitalOut(D5, 0);
+
+ DigitalOut(D10, 0);
+
+ DigitalOut(D11, 1);
+
+
+ }
+
+
+
+ void stop()
+
+
+{
-unsigned int dist;
-DigitalOut myled(LED1);
-DigitalOut myled1(LED2);
-DigitalOut myled2(LED3);
-int count = 0;
+ DigitalOut(D3,0);
-int main ()
- {
-
- while (1) // continuos loop to perform the scanning function of HCSR04 ..
- //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
- {
+ DigitalOut(D5, 0);
- servo.period(0.020);
- int distance = ping(); // ping function
- straight(); // use the ping() function to see if anything is ahead.
-
-
- if (distance < 10) // the condition for stopping the Obstacle Avoider...
- // ...whenever the defined proximity to obstacle is reached.
- {
- stop();
- scan();
-
-
- /*switch (a) // switch case for initial testing of the Obstacle detection by sensor and
- // Corresponding L.E.D. of the best direction will glow.
-
- {
- case 'r':
- myled=1;
- break;
- case 'l':
- myled1=1;
- break;
- case 'f':
- myled2=1;
- break;
- }*/
-
- }
- }
+ DigitalOut(D10,0);
+
+ DigitalOut(D11, 0);
+
+
}
-
- // A functon to perform the continuos pinging function of HCSR04 ..
- //a trigger pulse of Ultrasonic waves having duration 10 nS is sent through the echo pin
-
- int ping() // ultrasonic sensor is started and data collected is displayed on the LCD
- // ping function returns an int value of distance scanned
- {
- usensor.start();
- wait_ms(200);
- dist=usensor.get_dist_cm();
- lcd.cls();
- lcd.locate(0,0);
- if (dist <50)
- lcd.printf("cm:%ld",dist );
- else
- lcd.printf("no object");
- return (dist);
-
- }
+
+ /*
+ void loop()
+
+ {
+ servo.period(0.020);
+ usensor.start();
+ wait_ms(500);
+ distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
+ pc.printf("Dist: %d ",distance);
- void scan ()
- {
-
- int leftscanval, rightscanval; // variables for storing distance scan values...
- //... of nearest obstacle in the left and right direction
-
-
- // Scanning RIGHT side; turning the servo attached to HCSR04 right to check for obstacle distance
-
- // 0.0016 corresponds to the neutral position of servo motor
- // 0.0027 corresponds to the rightmost position of the servo motor
- // 0.0009 corresponds to the leftmost position of the servomotor
-
- for(float offset=0.0016; offset<=0.0027; offset+=0.0001) // turning RIGHT
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- wait (2); // stopping at RIGHTMOST position for 2 seconds
- rightscanval = ping(); // putting the distance of right direction's obstacle in ''rightscanval variable''
-
-
-
- for(float offset=0.0027; offset>=0.0016; offset+=-0.0001) // turning back to the CENTER position
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- wait(2); // stopping at CENTERMOST position for 2 seconds
+ //Check for objects...
+ if (distance > 50){
+ pc.printf(" No Object \n");
+ forward(); //All clear, move forward!
+ //noTone(buzzer);
+ // digitalOut LED(0);
+ }
+ else if (distance <=50){
+ pc.printf("Halt - Object on the Way \n");
+ stop();
+
+
+ for(float offset=0.0016; offset<=0.0020; offset+=0.0001) // turning RIGHT
+
+{
+
+ pc.printf("Turning Right");
+
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait_ms(0.10);
+
+}
+
+wait_ms(200); // stopping at RIGHTMOST position for 2 seconds
+
+
+ usensor.start();
+
+ wait_ms(500);
+
+ checkRight = usensor.get_dist_cm(); //Take distance from the left side
+
+ pc.printf("DistL: %d ",checkRight);
+
+
+ for (float offset=0.0020; offset>=0.0016;) // turning back to theCENTER position
+{
+ pc.printf("Turning Center");
+
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+offset=offset-0.0001;
+
+wait_ms(0.10);
+
+}
+
+wait_ms(200);
+
+
+for (float offset=0.0016; offset>=0.0012; offset=offset-0.0001) // Turning towards LEFT
+
+{
+
+ pc.printf("Turning Left");
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait_ms(0.10);
+
+}
+
+wait_ms(200); // stopping at LEFTMOST position for 2
+
+
+ usensor.start();
+
+ wait_ms(500);
+
+ checkLeft= usensor.get_dist_cm();
+
+ pc.printf("DistR: %d ",checkLeft);
+
+
+for(float offset=0.0012; offset<=0.0016; offset=offset+0.0001)
+
+{
+ pc.printf("Turning Center");
+
+servo.pulsewidth(offset); // servo position determined by a pulsewidth between
+
+wait_ms(0.10);
+
+}
+
+
+ //Finally, take the right decision, turn left or right?
+
+ if (checkLeft < checkRight){
+
+ backward();
+ wait_ms(1000);
+
+
+ left();
+pc.printf("goingleft");
+ wait_ms(400);
+
+ // wait_ms, change value if necessary to make robot turn.
+ forward();
+ pc.printf("now going straight");
+ }
+
+ else if (checkLeft > checkRight){
+
+ backward();
+ wait_ms(1000);
+
+ right();
- // Scanning LEFT side; turning the servo attached to HCSR04 right to check for obstacle distance
-
-
- for(float offset=0.0016; offset>=0.0009; offset-=0.0001) // Turning towards LEFT
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- wait(2); // stopping at LEFTMOST position for 2 seconds
- leftscanval = ping(); // putting the distance of right direction's obstacle in ''leftscanval variable''
-
-
- for(float offset=0.0009; offset<=0.0016; offset+=0.0001)
-
- {
- servo.pulsewidth(offset); // servo position determined by a pulsewidth between 1-2ms
- wait(0.10);
- }
- /*center scan servo (for changing the default position of Servo to the Centre Position ) This method didnt work
- though. There is a library of Servo
- */
- //servo.pulsewidth(0.00017);
-
-
- /* Comparisons to choose the most appropiate direction for the turning of Robot. Based on the distances stored in variables "leftscanvalue" and "rightscanvalue" */
-
- if (leftscanval>rightscanval )
-
- {
- left();
- }
-
- else if (rightscanval>leftscanval )
-
- {
- right();
- }
-
- else
- {
- left();
- }
-
- //return choice;
- }
-
- void stop () // function for stopping the DC motors
-
- {
- A.speed(0);
- B.speed(0);
- }
-
-
- void left() // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so...
- // ..that the robot turns to the LEFT
-
- {
- for (float s= -1.0; s < 0.0 ; s += 0.01)
- {
- A.speed(-1);
- B.speed(-1);
- wait(0.02);
- }
- }
-
-
- void right () // function for moving the RIGHT motor in REVERSE direction and LEFT motor in FORWARD direction so...
- // ..that the robot turns to the LEFT
-
- {
- for (float s= -1.0; s < 0.0 ; s += 0.01)
- {
- A.speed(1);
- B.speed(1);
- wait(0.02);
- }
+
+ wait_ms(400);
+ forward();
+ // wait_ms, change value if necessary to make robot turn.
+
}
+ else if (checkLeft <=10 && checkRight <=10){
- void straight () // function for moving the RIGHT motor in FORWARD direction and LEFT motor in REVERSE direction so...
- // ..that the robot turns to the LEFT
+ backward();
+ wait_ms(1000); //The road is closed... go back and then left ;)
- {
- //for (float s= -1.0; s < -0.7 ; s += 0.01)
- {
- A.speed(-1);
- B.speed(1);
- wait(0.02);
- }
-
- }
\ No newline at end of file
+ left();
+ wait_ms(400);
+ forward();
+
+ }
+
+ }
+*/
+
\ No newline at end of file