Hi arduino forum!
I have recently been working on a robot that uses an infrared sensor on a servo to avoid obstacles. I have been basing the code off of this guys:
http://letsmakerobots.com/node/1069
The only difference is that I have been using a infrared sensor instead of a ping, so I modified the code for this. The robot can detect obstacles and then back away from them, but then that is all it does. It gets caught in a loop of detecting the wall moving back and then moving forward again instead of turning left or right like I wanted it to do. Can somebody help me out with what is going wrong with the code?
#include <Servo.h>
int motor_left[] = {2,3 };
int motor_right[] = {4,5 };
int infrasensorPin = 1;
int infrasensorPin2 = 1;
int servoPin = 9;
int servoRange[] = {500, 2500};
int servoRotateTime = 500;
int dangerDist = 550;
int servoTime = 700;
int turnTime = 250;
int dist = analogRead(infrasensorPin);
int val = analogRead(infrasensorPin);
Servo servo;
void setup(){
// Setup motor
int i;
for(i = 0; i < 2; i++){
pinMode(motor_left[i], OUTPUT);
pinMode(motor_right[i], OUTPUT);
}
// Servo
servo.attach(9);
turn_servo(100);
}
void loop(){
int dist = (val);
if(dist > dangerDist){
drive_straight();
delay(50);
while(val == LOW) {
val = analogRead(infrasensorPin);
while(val == HIGH) {
val = analogRead(infrasensorPin);
}
} }else{
change_direction();
delay(50);
}
}
void change_direction(){
motor_stop();
turn_servo(180);
int left = analogRead (infrasensorPin);
turn_servo(10);
int right = analogRead(infrasensorPin2);
if(left < dangerDist && right < dangerDist){
turn_right();
delay(turnTime * 2);
drive_straight();
}
// Right is better
else if(left > right){
turn_right();
delay(turnTime);
drive_straight();
}
else {
turn_left();
delay(turnTime);
drive_straight();
}
}
void turn_servo(int degree){
int servoStopAt = 0;
if(servoStopAt == 0){
servoStopAt = millis() + servoRotateTime;
}
servo.write(degree);
while(servoStopAt > millis()){
delay(20);
}
}
/*
* Motor movements
*/
void motor_stop(){
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], HIGH);
}
void drive_straight(){
turn_servo(100);
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], LOW);
}
void turn_left(){
digitalWrite(motor_left[0], HIGH);
digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], HIGH);
digitalWrite(motor_right[1], LOW);
}
void turn_right(){
digitalWrite(motor_left[0], LOW);
digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], LOW);
digitalWrite(motor_right[1], HIGH);
}