#include <Servo.h>
#include <avr/interrupt.h>

Servo steering;
Servo throttle;
int bumpswitchPin = 2;
int forwardLED = 12;
int reverseLED = 11;
int bumpswitchState = 0;
int AccelerationSpeed = 105;
int ReverseSpeed = 75;
volatile int bumper;


void setup() {
  steering.attach(9);
  throttle.attach(10);
  pinMode(forwardLED, OUTPUT);
  pinMode(reverseLED, OUTPUT);
  pinMode(bumpswitchPin, INPUT);
  Serial.begin(115200);
  digitalWrite(bumpswitchPin, LOW);
  attachInterrupt(0, bumperISR, RISING);
  interrupts();
  bumper = 0;
  MoveForward();
}

void bumperISR(){
    digitalWrite(forwardLED, LOW);
    digitalWrite(reverseLED, LOW);
    Serial.println("Bumper Hit!!");
    Brakes();
    bumper = 1;
}

void Brakes() {
  throttle.write(0);
  Serial.println("Brakes on!!");
  Serial.println('\n');
}

void Stop() {
  throttle.write(90);
  Serial.println("Throttle at neutral");
  Serial.println('\n');
}

void MoveForward(){
  throttle.write(AccelerationSpeed);
  digitalWrite(forwardLED, HIGH);
  Serial.print("Throttle @ ");
  Serial.println(AccelerationSpeed);
  Serial.println('\n');
}

void loop() {
  steering.write(90);
  if (bumper > 0) {
    delay(200);
    Stop();
    delay(200);
    steering.write(55);
    delay(5);
    throttle.write(ReverseSpeed);
    digitalWrite(reverseLED, HIGH);
    Serial.print("Throttle @ ");
    Serial.println(ReverseSpeed);
    Serial.println('\n');
    delay(1350);
    Stop();
    digitalWrite(reverseLED, LOW);
    steering.write(90);
    delay(5);
    delay(500);
    bumper = 0;
    MoveForward();
 }
}