zaterdag 16 april 2011

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("");
}