Line follower

#include <Adafruit_NeoPixel.h>

int pwm_a = 3;   //PWM control for motor outputs 1 and 2 is on digital pin 3
int pwm_b = 11;  //PWM control for motor outputs 3 and 4 is on digital pin 11
int dir_a = 12;  //direction control for motor outputs 1 and 2 is on digital pin 12
int dir_b = 13;  //direction control for motor outputs 3 and 4 is on digital pin 13
int leftLED = 0;
int rightLED = 1;
int leftGND = 2;
int rightGND = 3;

int LED_COUNT = 1;
int RGBVCC = 10;
int RGBData = 9;
Adafruit_NeoPixel leds = Adafruit_NeoPixel(LED_COUNT, RGBData, NEO_RGB + NEO_KHZ800);

int vooruit_a = HIGH; // polariteit motor a
int vooruit_b = HIGH; // polariteit motor b

int zwartGrens = 600;
int draaiSnelheid = 150;
int snelheid = 150;
int vooruitTijd = 400;
int pauzetijd = 10;

void setup()
{
  pinMode(pwm_a, OUTPUT);  //Set control pins to be outputs
  pinMode(pwm_b, OUTPUT);
  pinMode(dir_a, OUTPUT);
  pinMode(dir_b, OUTPUT);
  pinMode(leftLED, OUTPUT);
  pinMode(rightLED, OUTPUT);

  // Stel RGB LED voeding en data in
  pinMode(RGBVCC, OUTPUT);
  digitalWrite(RGBVCC, HIGH);
  pinMode(RGBData, OUTPUT);
  leds.begin();
  leds.clear();
  leds.show();

  // Maak extra GND voor LED's
  pinMode(leftGND, OUTPUT);
  pinMode(rightGND, OUTPUT);
  digitalWrite(leftGND, LOW);
  digitalWrite(rightGND, LOW);

  // Zet beide motoren stil
  analogWrite(pwm_a, 0);
  analogWrite(pwm_b, 0);

  // Stel de seriele poort in om de sensor data te ontvangen
  //Serial.begin(9600);
}

void loop()
{
  digitalWrite(leftLED, LOW);
  digitalWrite(rightLED, HIGH);
 
  leds.setPixelColor(0, 0xff0000);
  leds.show();

  // Draai de zwarte lijn op
  int sensorVal = analogRead(A0);
  if(sensorVal < zwartGrens) {
    // Start rond te draaien
    rond(true);

    // Draai rond totdat de lijn gevonden is
    do {
      sensorVal = analogRead(A0);
      //Serial.println(sensorVal);
    } while(sensorVal < zwartGrens);

    // Stop met draaien zodra de lijn gevonden is
    //stoppen();
    delay(pauzetijd);
  }

  leds.setPixelColor(0, 0x0000ff);
  leds.show();

  digitalWrite(leftLED, HIGH);
  digitalWrite(rightLED, HIGH);
 
  // Ga vooruit
  vooruit();

  // Blijf een tijdje vooruit gaan of totdat de lijn verlaten wordt
  int start = millis();
  int duur;
  do {
      sensorVal = analogRead(A0);
      duur = millis() - start;
  } while(sensorVal > zwartGrens && duur < vooruitTijd);
  //stoppen();
  delay(pauzetijd);

  digitalWrite(leftLED, HIGH);
  digitalWrite(rightLED, LOW);
 
  leds.setPixelColor(0, 0x00ff00);
  leds.show();

  // Draai even van de zwarte lijn af
  rond(false);
  sensorVal = analogRead(A0);
  if(sensorVal > zwartGrens) {
    // Start rond te draaien
    rond(false);

    // Draai rond totdat de lijn gevonden is
    do {
      sensorVal = analogRead(A0);
      //Serial.println(sensorVal);
    } while(sensorVal > zwartGrens);

    // Stop met draaien zodra de lijn gevonden is
    //stoppen();
    delay(pauzetijd);
  }
}

void stoppen() {
  // Niet meteen stoppen zodat de sensorwaarden een klein stukje voorbij gereden worden...
  delay(50);
 
  analogWrite(pwm_a, 0);
  analogWrite(pwm_b, 0);

  // altijd even een tijdje stil staan zodat je kan volgen wat er gebeurt
  delay(1000);
}

/*
 * richting, true om met de klok mee te draaien. False voor tegen de klok in.
 */
void rond(boolean richting) {
  digitalWrite(dir_a, vooruit_a xor richting);
  digitalWrite(dir_b, !(vooruit_b xor richting));

  analogWrite(pwm_a, motorenAan()?draaiSnelheid:0);
  analogWrite(pwm_b, motorenAan()?draaiSnelheid:0);
}

void vooruit() {
  digitalWrite(dir_a, vooruit_a);
  digitalWrite(dir_b, vooruit_b);

  analogWrite(pwm_a, motorenAan()?snelheid:0);
  analogWrite(pwm_b, motorenAan()?snelheid:0);
}

boolean motorenAan() {
  int val = analogRead(A5);
  return val > 300;
}