I am using a adafuit motor sheild, 4 wheel base, 4 pin ultrasonic distance sensor, with an Arduino Uno. I am not using a servo but I am willing to. I have the trig pin on 10 and the echo pin on 2 I need a better code. this is the code I am currently using, which is not working well:
#define trigPin 10
#define echoPin 2
#include <AFMotor.h>
int sound = 250;
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
Serial.begin(115200);
motor1.setSpeed(200);
motor2.setSpeed(200);
motor3.setSpeed(200);
motor4.setSpeed(200);
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
void loop() {
uint8_t i;
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance <= 3) {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
delay(10);
}
else {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
delay(10);
}
}
I would like the code to include left, right, forward and reverse.
Thank you
-Ben