#include <Servo.h>

Servo left;
Servo right;

int leftPin = 10;
int rightPin = 9;

int leftCentre = 91;
int rightCentre = 91;

long volatile lastBumpLeft = 0;
boolean volatile edgeLeft = true;

long volatile lastBumpRight = 0;
boolean volatile edgeRight = true;

void setup() {
  Serial.begin(9600);
  Serial.println("BoxBox HWTM");
  
  left.attach(leftPin);
  right.attach(rightPin);
  stop();
  
  attachInterrupt(0, bumpLeft, CHANGE);
  attachInterrupt(1, bumpRight, CHANGE);
}

void loop() {
  stop();
  Serial.println("Both wheels stopped");
  delay(2000);
  
  leftForward();  
  Serial.println("Left wheel forward");
  delay(2000);
  leftStop();
  
  rightForward();
  Serial.println("Right wheel forward");
  delay(2000);
  rightStop();
  
  leftBackward();
  Serial.println("Left wheel backwards");
  delay(2000);
  leftStop();
  
  rightBackward();
  Serial.println("Right wheel backwards");
  delay(2000);
  rightStop();
  
  forward();
  Serial.println("Both wheels forwards");
  delay(2000);
  stop();
  
  backward();
  Serial.println("Both wheels backwards");
  delay(2000);
  stop();
  
  Serial.println("Finished.");
  Serial.println("You should test the bump sensors now");
  
  for(;;);
}

void forward() {
  leftForward();
  rightForward();
}

void leftForward() {
  left.write(0);
}

void rightForward() {
  right.write(180);
}

void backward() {
  leftBackward();
  rightBackward();
}

void leftBackward() {
  left.write(180);
}

void rightBackward() {
  right.write(0);
}

void stop() {
  leftStop();
  rightStop();
}

void leftStop() {
  left.write(leftCentre);
}

void rightStop() {
  right.write(rightCentre);
}

void bumpLeft() {
  long time = millis();
  if ((time - lastBumpLeft) > 100L) {
    if (digitalRead(2) == HIGH) {
      Serial.println("Bump Left!!");
    }
  }
  lastBumpLeft = time;
}

void bumpRight() {
  long time = millis();
  if ((time - lastBumpRight) > 100L) {
    if (digitalRead(3) == HIGH) {
      Serial.println("Bump Right!!");
    }
  }
  lastBumpRight = time;
}