#define trigPin 12 // define the pins of your sensor
#define echoPin 13
#define m1 6
#define m2 5
#define m3 9
#define m4 10
void setup() {
Serial.begin(9600); // begin serial communitication
Serial.println("Motor test!");
pinMode(trigPin, OUTPUT);// set the trig pin to output (Send sound waves)
pinMode(echoPin, INPUT);// set the echo pin to input (recieve sound waves)
pinMode(m1, OUTPUT);
pinMode(m2, OUTPUT);
pinMode(m3, OUTPUT);
pinMode(m4, OUTPUT);
}
void loop() {
long duration, distance; // start the scan
digitalWrite(trigPin, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); //this delay is required as well!
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;// convert the distance to centimeters.
if (distance < 25)/*if there's an obstacle 25 centimers, ahead, do the following: */ {
Serial.println ("Close Obstacle detected!" );
Serial.println ("Obstacle Details:");
Serial.print ("Distance From Robot is " );
Serial.print ( distance);
Serial.print ( " CM!");// print out the distance in centimeters.
Serial.println (" The obstacle is declared a threat due to close distance. ");
Serial.println (" Turning !");
//delay(500);
analogWrite(m3,80);
analogWrite(m4,80);
analogWrite(m1,0);
analogWrite(m2,0);
delay(1000);
analogWrite(m1,100);
analogWrite(m2,0);
analogWrite(m3,0);
analogWrite(m4,0);
}
else {
Serial.println ("No obstacle detected. going forward");
delay (15);
analogWrite(m1,100);
analogWrite(m2,100);
}
//delay(500);
}
No comments