#include <AFMotor.h>
const int trigPin = A2;
const int echoPin = A3;
long duration;
int distance;
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR12_1KHZ);
void setup() {
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
Serial.begin(9600);
motor2.setSpeed(250);
motor3.setSpeed(250);
}
void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance= duration*0.034/2;
Serial.print("Distance: ");
Serial.println(distance);
if(distance < 75)
{
backward();
delay(1000);
movestop();
delay(1000);
lookright();
delay(200);
}
else
{
forward();
delay(1000);
}
}
void movestop()
{
motor2.run(RELEASE);
motor3.run(RELEASE);
}
void forward()
{
motor2.run(FORWARD);
motor3.run(FORWARD);
motor2.setSpeed(200);
motor3.setSpeed(200);
}
void backward()
{
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor2.setSpeed(200);
motor3.setSpeed(200);
}
void lookright()
{
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor2.setSpeed(200);
motor3.setSpeed(200);
}
void lookleft()
{
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor2.setSpeed(200);
motor3.setSpeed(200);
}
Terkait