Robotje poging 0.2

dit is een verbeterde versie van de controller software.
De ir sensor zijn ook dichter bij elkaar gezet, zo kijkt de robot iets meer voor zicht uit.

// left motor is on pin 3 and 4 of motorboard
int l_direction = 7;      // select the pin for the E2 Left motor direction
int l_motor     = 6;      // select the pin for the M2 Left motor speed
int l_sensorPin = 1;    // select the input pin for the sensor
int l_sensorValue = 0;  // variable to store the value coming from the sensor
boolean l_directionValue = HIGH;  // variable to store the direction

// right motor is on pin 1 and 2 of motorboard
int r_direction = 4;      // select the pin for the E1 Right motor direction
int r_motor     = 5;      // select the pin for the M1 Right motor speed
int r_sensorPin = 0;    // select the input pin for the sensor
int r_sensorValue = 0;  // variable to store the value coming from the sensor
boolean r_directionValue = HIGH;  // variable to store the direction

#define button 2   //frontbutton
boolean front;
unsigned long int timer_reversing;
unsigned long int timer_turning;
boolean reversing;
boolean turning;
char turndirection;

void setup() {
  pinMode(l_direction, OUTPUT);
  pinMode(r_direction, OUTPUT);
  pinMode(button,INPUT);
  reversing = false;
  turning = false;
  turndirection = 'n';
//  Serial.begin(57600);
}

void loop() {
  l_sensorValue = readsensor(l_sensorPin);
  r_sensorValue = readsensor(r_sensorPin);

  l_directionValue = setDirection(l_sensorValue);
  r_directionValue = setDirection(r_sensorValue);
  
  front = digitalRead(button);
  if (front == HIGH) {
    l_directionValue = LOW;      
    r_directionValue = LOW;      
  }
  if(l_sensorValue < 20 && r_sensorValue < 20) {
    start_reverse();
  }  
  turn();
  
//  serialprint(l_sensorValue,l_directionValue,r_sensorValue,r_directionValue);
  
  controlMotor('l',l_sensorValue,l_directionValue);
  controlMotor('r',r_sensorValue,r_directionValue);
}

void start_reverse(){
  if (reversing == false) {
    timer_reversing = millis() + 1000;
    reversing = true;
  }
}
void start_turn() {
  if (turning == false) {
    timer_turning = millis() + 500;
    turning = true;
  }
}
void reverse() {
  if (timer_reversing < millis() ) {
    if(reversing == true) {
      start_turn();
      if(l_sensorValue < r_sensorValue) {
        //turn left
        turndirection = 'l';
      } else {
        // turn right
        turndirection = 'r';
      }
    }
    reversing = false;
  }
}
void turn() {
  if (turning == true && reversing == false) {
    switch (turndirection) {
      case 'l':
        //turn left
        l_directionValue = LOW;
        r_directionValue = HIGH;
        break;
      case 'r':
        // turn right
        l_directionValue = HIGH;
        r_directionValue = LOW;
      default:
        l_directionValue = HIGH;
        r_directionValue = HIGH;
    }
  }
  if (timer_turning < millis() ) {
    turning = false;
  }
}

boolean setDirection(int value) {
    if (value > 0) {
    return HIGH;
  } else {
    return LOW;
  }
}
int readsensor(int sensor) {
  int value = map(analogRead(sensor), 0, 512, 255, 0);
  return value;
}

void controlMotor(char side, int value, boolean directionValue){
  reverse();
  if (reversing == true){
    directionValue = LOW;
  }
  switch(side){
    case 'r':
      analogWrite(l_motor,value);  
      digitalWrite(l_direction,directionValue);
      break;
    case 'l':
      analogWrite(r_motor,value);  
      digitalWrite(r_direction,directionValue);
      break;
  }
}

void serialprint(int l_s, int l_d, int r_s, int r_d) {
  Serial.print("l = ");
  Serial.print(l_s);
  Serial.print("  Dir = ");
  Serial.print(l_d);
  Serial.print("     r = ");
  Serial.print(r_s);
  Serial.print("  Dir = ");
  Serial.print(r_d);
  Serial.println("");
}