Arduino Robot Code

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);
}
 int servoStopAt = 0;

  
 if(servoStopAt == 0){

Why bother with a conditional?

I see several problems with this code. First is this:

 int dist = analogRead(infrasensorPin);
 int val = analogRead(infrasensorPin);

You can't have executable code outside of a function, like this. The dist and val variables are not valued. In addition, val is never changed again.

Then, there's this, in loop:

  int dist = (val);

The parentheses are not needed, and you've created a local variable of the same name as a global variable, and assigned it a value of 0.

Even if you fix that problem, you have this code:

 if(dist > dangerDist){
   drive_straight();
   delay(50);
   while(val == LOW) {
   val = analogRead(infrasensorPin);
    while(val == HIGH) {
   val = analogRead(infrasensorPin);
 }

This code points the servo to 100 degrees (that's a strange value for straight ahead), and starts the motors running. It does not take a sensor reading to make sure that it is safe to charge ahead. The robot charges ahead for 50 milliseconds blindly. Then, it reads the sensor until it goes LOW, while still rolling straight ahead.

Once the sensor goes LOW, it keeps rolling straight ahead until the sensor goes HIGH.

You have not identified how the sensor is connected, so whether HIGH or LOW means that the sensor detected something is not clear.

It's probably good that this code is never executed.

In change_direction, which is called, you turn the servo to 180 and take a reading. That's an 80 degree turn to the left. Then, you move the servo to 10 and take a reading. That's a 90 degree turn to the right (based on 100 being straight ahead).

Why the asymmetrical readings? Why do you not take a reading straight ahead?

If there is an obstacle to the left (left < dangerDist) and an obstacle to the right (right < dangerDist), you turn right for a while, then aim the sensor straight ahead and drive straight (towards the obstacle).

If the obstacle on the left is farther away than the one on the right, you turn right (towards the nearer obstacle). Otherwise, you turn left (again, towards the nearer obstacle).

If your intent is to hit the most obstacles, your on the right path.

The turn_servo code needs some work, too.

int servoStopAt = 0;
 if(servoStopAt == 0){
   servoStopAt = millis() + servoRotateTime;
 }

How can servoStopAt be anything other than 0? Adding a value to the output from millis() is not a good idea.

  servo.write(degree);

OK. This moves the servo, in one big move.

 while(servoStopAt > millis()){
   delay(20);
 }

You know how long you want to delay for (servoRotateTime). Why not just delay that long? What is the advantage of waiting in 20 millisecond increments?

Alright, I get it, the code isn't the greatest and I should probably scrap it and start over. Before I begin though I have a question. I was using a sharp IR sensor on analog pin one. If once the sensor detects something in front of it and the servo turns to the left and the right, how would I write the code to store the the values of each side and then compare them? Would I make two separate values for the one sensor? Or some way of storing the two values on the arduino? I was looking at the analog IO section of the arduino learning section and couldn't seem to find exactly what I was looking for.

You have code now that does this:

 int left = analogRead (infrasensorPin);
 int right = analogRead(infrasensorPin[glow]2[/glow]);

By the way, is that right? How many sensors do you have? I got the impression that the sensor was being moved by the servo.

[edit]Never mind, I see that both variables have the same value. You may want to consider getting rid of one of them, though.[/edit]

Scrapping the code and starting over is not necessary. You really only have a few things wrong. In change_direction, you need to position the servo to point the sensor to the left, and read it (done). Then, you need to position the servo to point the sensor to the right, and read it (done). Finally, you should move the servo to point the sensor forward, and read it (not done). Then, go left, forward, or right, based on which direction provided the greatest distance to an obstruction.

You should also consider backing up, in case there is no good direction forward.

Getting all this right is not trivial. You've already got most of it working. There is a little bit that needs work. Don't let my long response discourage you. You've done a lot of good stuff. I'd say you were about 80% there.