Arduino Roboter

Nun habe ich endlich mein neues Spielzeug erhalten und musste auch gleich damit beginnen einen “LineFollower” zu basteln. Hier mal ein Video vom fertigen Roboter:

Roboter

Schematischer Aufbau

Und anschließend noch der Code:

//motor RIGHT connected between A01 and A02
//motor LEFT connected between B01 and B02

int STBY = 10; //standby

//Motor A
int PWMA = 3; //Speed control 
int AIN1 = 9; //Direction
int AIN2 = 8; //Direction

//Motor B
int PWMB = 5; //Speed control
int BIN1 = 11; //Direction
int BIN2 = 12; //Direction

int RIGHT = 2;
int LEFT = 1;
int QRE1113_LEFT = 2;
int QRE1113_RIGHT = 4;
int schwellwert = 400;
void setup(){
  pinMode(STBY, OUTPUT);

  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);

  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  
  Serial.begin(9600);
  stop();
  delay(5000);
}

void loop(){
  // Lesen der Sensorwerte
  Serial.println("--");  
  int QRE_Value_LEFT = readQD(LEFT);
  Serial.println(QRE_Value_LEFT); 
  int QRE_Value_RIGHT = readQD(RIGHT);
  Serial.println(QRE_Value_RIGHT);  
  // Falls beide Sensoren schwarzen Untergrund liefern geradeaus
  // fahren
  if(QRE_Value_LEFT >= schwellwert && QRE_Value_RIGHT >= schwellwert)
  {
    Serial.println("forward");
    move(LEFT, 200, 1);
    move(RIGHT, 200, 1);
  } else if (QRE_Value_LEFT < schwellwert && QRE_Value_RIGHT< schwellwert) {
    stop(); // Überarbeiten
    if(QRE_Value_LEFT<QRE_Value_RIGHT){
      Serial.println("sharp RIGHT");
      while(readQD(RIGHT)<schwellwert) {
        move(LEFT, 150, 1);
        move(RIGHT, 150, 0);
      }
    } else {
      Serial.println("sharp LEFT");
      while(readQD(LEFT)<schwellwert) {
        move(LEFT, 150, 0);
        move(RIGHT, 150, 1);
      }
    }
    stop();
  } else {
    if(QRE_Value_LEFT<QRE_Value_RIGHT){
      Serial.println("RIGHT");
      move(RIGHT, 100, 1);
      move(LEFT, 200, 1);
    } else {
      Serial.println("LEFT");
      move(LEFT, 100, 1);
      move(RIGHT, 200, 1);
    }
  }
  delay(5);
}
int readQD(int motor){
  //Returns value from the QRE1113 
  //Lower numbers mean more refleacive
  //More than 3000 means nothing was reflected.
  int QRE1113_Pin = QRE1113_RIGHT;
  if(motor == LEFT){
    QRE1113_Pin = QRE1113_LEFT;
  }
  pinMode( QRE1113_Pin, OUTPUT );
  digitalWrite( QRE1113_Pin, HIGH );
  delayMicroseconds(10);
  pinMode( QRE1113_Pin, INPUT );

  long time = micros();

  //time how long the input is HIGH, but quit after 3ms as nothing happens after that
  while (digitalRead(QRE1113_Pin) == HIGH && micros() - time < 3000);
  int diff = micros() - time;
  pinMode( QRE1113_Pin, OUTPUT );
  digitalWrite( QRE1113_Pin, LOW );
  return diff;
}

void move(int motor, int speed, int direction){
  //Move specific motor at speed and direction
  //motor: 0 for B 1 for A
  //speed: 0 is off, and 255 is full speed
  //direction: 0 clockwise, 1 counter-clockwise
  digitalWrite(STBY, HIGH); //disable standby

  boolean inPin1 = LOW;
  boolean inPin2 = HIGH;

  if(direction == 1){
    inPin1 = HIGH;
    inPin2 = LOW;
  }

  if(motor == LEFT){
    digitalWrite(AIN1, inPin1);
    digitalWrite(AIN2, inPin2);
    analogWrite(PWMA, speed);
  }else{
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);
  }
}

void stop(){
  //enable standby  
  digitalWrite(STBY, LOW);
  move(LEFT, 0, 1);
  move(RIGHT, 0, 1);
}