Download (PDF, 685KB)

//motor a, placed on the right
#define brakeR 9
#define powerR 3
#define direcR 12
int powValR;

//motor b, placed on the left
#define brakeL 8
#define powerL 11
#define direcL 13
int powValL;

//motor speeds
const int quatS = 63;
const int halfS = 123;
const int thqtS = 191;
const int fullS = 255;

//switch for the motor
#define start 2
int switchVal;
bool motorOn = false;

//general control
int lastMsg = -1;
bool firstPart = true;    //following the line
bool secondPart = false;  //approaching the wall
bool thirdPart = false;   //driving along the wall to the goal

//sensors
#define lightR A0    //right light sensor
#define lightL A5    //left light sensor
#define frUlSI A2    //front ultrasound sensor input (echo)
#define frUlSO 5     //front ultrasound sensor output (trigger)
#define sdUlSI A3    //right side ultrasound sensor input (echo)
#define sdUlSO 6     //right side ultrasound sensor output (trigger)
const int threshold = 45; 
int litValR, litValL, litDif;
long dur, frDist, sdDist;

void setup() {
  pinMode(direcR, OUTPUT);
  pinMode(direcL, OUTPUT);
  pinMode(brakeR, OUTPUT);
  pinMode(brakeL, OUTPUT);
  pinMode(powerR, OUTPUT);
  pinMode(powerL, OUTPUT);
  
  pinMode(start, INPUT);
  pinMode(lightR, INPUT);
  pinMode(lightL, INPUT);
  pinMode(frUlSO, OUTPUT);
  pinMode(frUlSI, INPUT);
  pinMode(sdUlSO, OUTPUT);
  pinMode(sdUlSI, INPUT);

  Serial.begin(9600);
}

void loop() {
  while(firstPart) {
    checkSwitch();
    
    if(motorOn) {
      digitalWrite(brakeR, LOW);
      digitalWrite(brakeL, LOW);
  
      litValR = analogRead(lightR);
      litValL = analogRead(lightL);
      litDif = litValR - litValL;
  
      if(litDif >= -(threshold) && litDif <= threshold) {
        //all wheels running forward
        digitalWrite(direcR, LOW);
        digitalWrite(direcL, HIGH);
        
        if(lastMsg != 0) {
          lastMsg = 0;
        }
        powValR = fullS;
        powValL = fullS;
      }
      else if (litDif < -(threshold)) {      
        if(lastMsg != 1) {
          brake();
          digitalWrite(brakeR, LOW);
          digitalWrite(brakeL, LOW);
          
          lastMsg = 1;
        }
        //right wheels runs backwards
        digitalWrite(direcR, LOW);
        digitalWrite(direcL, LOW);
        
        //turn left
        powValR = fullS;
        powValL = halfS;
      }
      else if (litDif > threshold) {
        if(lastMsg != 2) {
          brake();
          digitalWrite(brakeR, LOW);
          digitalWrite(brakeL, LOW);
          
          lastMsg = 2;
        }
        
        //left wheels runs backwards
        digitalWrite(direcR, HIGH);
        digitalWrite(direcL, HIGH);
        //turn right
        powValR = halfS;
        powValL = fullS;
      }
      else {
        Serial.println("ERROR: Something in the math went wrong!");
      }
  
      analogWrite(powerR, powValR);
      analogWrite(powerL, powValL);
    }
    else {
      digitalWrite(brakeR, HIGH);
      digitalWrite(brakeL, HIGH);
  
      analogWrite(powerR, 0);
      analogWrite(powerL, 0);
    }

    if(litValR > 850 && litValL > 850) {
      firstPart = false;
      secondPart = true;
      brake();
    }
  }

  delay(200);

  while(secondPart) {
    checkSwitch();

    frDist = measureDist(frUlSI, frUlSO);

    if(motorOn) {
      //Med/uden ledning: 15/10
      while(frDist > 12 && motorOn) {
        checkSwitch();

        frDist = measureDist(frUlSI, frUlSO);

        digitalWrite(brakeR, LOW);
        digitalWrite(brakeL, LOW);
        digitalWrite(direcR, LOW);
        digitalWrite(direcL, HIGH);
        powValR = fullS;
        powValL = fullS;
        analogWrite(powerR, powValR);
        analogWrite(powerL, powValL);
      }

      brake();
      delay(200);

      while(frDist < 100 && motorOn) {
        checkSwitch();

        frDist = measureDist(frUlSI, frUlSO);

        digitalWrite(brakeR, LOW);
        digitalWrite(brakeL, LOW);
        digitalWrite(direcR, LOW);
        digitalWrite(direcL, LOW);
        powValR = fullS;
        powValL = halfS;
        analogWrite(powerR, powValR);
        analogWrite(powerL, powValL);
      }

      //Med/uden ledning: 500/300
      delay(600);
      brake();

      secondPart = false;
      thirdPart = true;
    }
  }

  delay(200);

  while(thirdPart) {
    checkSwitch();
  
    sdDist = measureDist(sdUlSI, sdUlSO);

    if(motorOn) {
      digitalWrite(brakeR, LOW);
      digitalWrite(brakeL, LOW);

      //Med/uden ledning: 10/8
      if(sdDist > 10) {
        //drej ind mod væg
        digitalWrite(direcR, HIGH);
        digitalWrite(direcL, HIGH);
        powValR = thqtS;
        powValL = fullS;
      }
      //Med/uden ledning: 8/6
      else if (sdDist < 8) {
        //drej væk fra væg
        digitalWrite(direcR, LOW);
        digitalWrite(direcL, LOW);
        powValR = thqtS;
        powValL = halfS;
      }
      else {
        //kør lige ud
        digitalWrite(direcR, LOW);
        digitalWrite(direcL, HIGH);
        powValR = thqtS;
        powValL = thqtS;
      }
      
      analogWrite(powerR, powValR);
      analogWrite(powerL, powValL);
    }
  }
  
  checkSwitch();
}

void checkSwitch() {
  switchVal = digitalRead(start);
  
  if(switchVal > 0) {
    if(motorOn) {
      motorOn = false;
    }
    else {
      motorOn = true;
    }

    firstPart = true;
    secondPart = false;
    thirdPart = false;

    delay(200);
    while(digitalRead(start) > 0);
  }
}

void brake() {
  analogWrite(powerR, 0);
  analogWrite(powerL, 0);

  digitalWrite(brakeR, HIGH);
  digitalWrite(brakeL, HIGH);
}

int measureDist(int echoPin, int trigPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  dur = pulseIn(echoPin, HIGH);
  return (dur / 2) / 29.1;  //converts to cm  
}