Hi there. I'm building an obstacle avoiding robot, and it uses an ultrasonic sensor to tell where it's at and where to turn. When it detects it's close to something, it looks left and right then goes to the direction of the greatest distance. The problem is, only one wheel really turns when it is making a direction change. I can't find the error. If you can help it would be great. Keep in mind that it uses an H-bridge with two DC motors.
"#include <Servo.h>
Servo myservo;
int pos=0;
int trigPin=3;
int echoPin=2;
int leftmf=10;
int leftmb=11;
int rightmf=8;
int rightmb=11;
void setup()
{
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(13, OUTPUT);
pinMode(leftmf, OUTPUT);
pinMode(leftmb, OUTPUT);
pinMode(rightmf, OUTPUT);
pinMode(rightmb, OUTPUT);
digitalWrite(trigPin, LOW);
myservo.attach(4);
}
void loop()
{
digitalWrite(rightmf, HIGH);
digitalWrite(rightmb, LOW);
digitalWrite(leftmf, HIGH);
digitalWrite(leftmb, LOW);
int duration, distance;
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH, 32760);
distance=(duration/2) / 29.1;
Serial.print(distance);
Serial.println("cm");
delay(100);
if (distance<20)
{
digitalWrite(rightmf, LOW);
digitalWrite(rightmb, LOW);
digitalWrite(leftmf, LOW);
digitalWrite(leftmb, LOW);
digitalWrite(13, HIGH);
for (pos=90; pos<180; pos+=1)
{
myservo.write(pos);
delay(5);
}
int leftdistance;
digitalWrite(trigPin, HIGH);
delayMicroseconds(15);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH, 32760);
leftdistance=(duration/2) / 29.1;
Serial.print("left distance=");
Serial.println(leftdistance);
delay(500);
for (pos=180; pos>=1; pos-=1)
{
myservo.write(pos);
delay(5);
}
int rightdistance;
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH, 32760);
rightdistance=(duration/2) / 29.1;
Serial.print("right distance=");
Serial.println(rightdistance);
delay(500);
for(pos=0; pos<=90; pos+=1)
{
myservo.write(pos);
delay(5);
}
if (rightdistance>leftdistance);
{
digitalWrite(rightmf, LOW);
digitalWrite(rightmb, HIGH);
digitalWrite(leftmf, HIGH);
digitalWrite(leftmb, LOW);
delay(250);
digitalWrite(rightmf, HIGH);
digitalWrite(rightmb, HIGH);
digitalWrite(leftmf, HIGH);
digitalWrite(leftmb, HIGH);
digitalWrite(13, LOW);
}
}
else
{
digitalWrite(rightmf, HIGH);
digitalWrite(rightmb, LOW);
digitalWrite(leftmf, LOW);
digitalWrite(leftmb, HIGH);
delay(250);
digitalWrite(rightmf, HIGH);
digitalWrite(rightmb, LOW);
digitalWrite(leftmf, HIGH);
digitalWrite(leftmb, LOW);
digitalWrite(13, LOW);
}
}