/* This sketch reads a HC-SR04 ultrasonic rangefinder and returns the distance to the closest object in range. To do this, it sends a pulse to the sensor to initiate a reading, then listens for a pulse to return. The length of the returning pulse is proportional to the distance of the object from the sensor. The circuit: * +V connection of the HC-SR04 attached to +5V * GND connection of the HC-SR04 attached to ground * SIG connection of the HC-SR04 attached to a digital pin Dincer Hepguler 2018 modified from original Ping example: http://www.arduino.cc/en/Tutorial/Ping */ #define En1 3 #define Mot1A 4 #define Mot1B 5 #define En2 6 #define Mot2A 7 #define Mot2B 8 #define pingPin 9 //both signal pins of sensor to pin9 #define sensor0 A0 //front gap sensor #define sensor1 A1 //back bump sensors int leftspeed=0; int rightspeed=0; int hiz=0; int leftDistance, rightDistance; long duration, cm; void setup() { Serial.begin(9600); pinMode(3,OUTPUT); pinMode(4,OUTPUT); pinMode(5,OUTPUT); pinMode(6,OUTPUT); pinMode(7,OUTPUT); pinMode(8,OUTPUT); digitalWrite(En1,LOW); digitalWrite(En2,LOW); } void loop() { // read IR sensors byte front= analogRead(sensor0); byte bump= analogRead(sensor1); // read the HC-SR4 delay(50); distance(); hiz= map(cm, 0, 400, 100, 250); Serial.print(cm); Serial.print("cm"); Serial.println(); if ((cm > 20) && (front < 200)){ forward(); delay(100); } else { avoid(); } if (bump <200){getFree();} } void distance(){ pinMode(pingPin, OUTPUT); digitalWrite(pingPin, LOW); delayMicroseconds(2); digitalWrite(pingPin, HIGH); delayMicroseconds(5); digitalWrite(pingPin, LOW); pinMode(pingPin, INPUT); duration = pulseIn(pingPin, HIGH); cm = duration /29/2; if (cm>400){cm=400;} return cm; } void forward(){ analogWrite(En1,hiz);digitalWrite(Mot1A,HIGH);digitalWrite(Mot1B,LOW); analogWrite(En2,hiz);digitalWrite(Mot2A,LOW);digitalWrite(Mot2B,HIGH); } void backward(){ analogWrite(En1,hiz);digitalWrite(Mot1A,LOW);digitalWrite(Mot1B,HIGH); analogWrite(En2,hiz);digitalWrite(Mot2A,HIGH);digitalWrite(Mot2B,LOW); } void left(){ analogWrite(En1,hiz);digitalWrite(Mot1A,LOW);digitalWrite(Mot1B,HIGH); analogWrite(En2,hiz);digitalWrite(Mot2A,LOW);digitalWrite(Mot2B,HIGH); } void right(){ analogWrite(En1,hiz);digitalWrite(Mot1A,HIGH);digitalWrite(Mot1B,LOW); analogWrite(En2,hiz);digitalWrite(Mot2A,HIGH);digitalWrite(Mot2B,LOW); } void halt(){ digitalWrite(En1,LOW); digitalWrite(En2,LOW); } void avoid(){ halt(); backward(); delay(1000); halt(); hiz=200; left(); delay(500); distance(); leftDistance = cm; //scan to the left and get left clearance delay(100); right(); delay(1000); distance(); rightDistance = cm; //scan to the right and get right clearance delay(100); if (leftDistance > rightDistance) //if left is less obstructed { left(); delay(1000); //Serial.print("LeftDist: "); //Serial.print(leftDistance); //Serial.println("cm"); } else { right(); //if right is less obstructed delay(1000); //Serial.print("RightDist: "); //Serial.print(rightDistance); //Serial.println("cm"); } } void getFree(){ forward(); delay(1000); halt(); hiz=200; left(); delay(500); distance(); leftDistance = cm; //scan to the left and get left clearance delay(100); right(); delay(1000); distance(); rightDistance = cm; //scan to the right and get right clearance delay(100); if (leftDistance > rightDistance) //if left is less obstructed { left(); delay(1000); } else { right(); //if right is less obstructed delay(1000); } }