// This code uses a button, led, and switches with servos. Hit the button
// once it puts the servos(robot arm) in standby mode, hit the button a second
// time it buts the robot arm in cycle mode and hit the button a third TIME 
// it turns the robot back off. While the robot arm is in cycle mode flip the 
// left switch to make it move. The original code used two break beam sensors 
// that are not availble on this simulator so I used the switches. In cycle mode
// flip the left swich on to simulate trigger a break beam sensor, leave it on
// and the robot will keep cycling and flip the right switch to simulate an 
// object blocking the robot destination or where it's going to set an object 
// down. It will not move until the path is clear. Alse the original code use
// VarSpeedServo library which would control the servo speed and gives a person
// an option to complete the movement before going to the next movement. I had 
// had a bunch of delays in this examble. The led flashes for warning before
// the robot moves and stays on while its moving.


#include <Servo.h>
const byte SENSORPIN1 = 1;
const byte SENSORPIN2 = 2;
const byte BUTTONPIN = 9;
const byte LEDPIN = 3;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

boolean blinkState = LOW;
boolean ledState = LOW;
boolean ledDone;
boolean event;
boolean robotCycle;
boolean robotWarning;
boolean robotEnable;
boolean robotMove;

int sensorState1 = 0, lastState1 = 0;
int sensorState2 = 0, lastState2 = 0;
int buttonPushCounter = 0;
int buttonState = 0;
int lastButtonState = 0;
int robotMoveCount = 0;
int robotMoveDone;

unsigned long robotWarningTime;
unsigned long debounceDuration = 50;
unsigned long lastButtonStateChange = 0;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
unsigned long time1 = 0;
unsigned long time2 = 0;
unsigned long time3 = 0;

const long interval = 100;

void setup() {
  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(7);
  servo5.attach(8);
  pinMode(BUTTONPIN, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  pinMode(SENSORPIN1, INPUT_PULLUP);
  pinMode(SENSORPIN2, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  blinkState = HIGH;
  event = true;
  robotMove = true;
  ledDone = true;
  robotWarning = false;
}

void loop() {
  currentMillis = millis();
  control_switch();
  led_blink();
  robot();
}

void control_switch() {
  sensorState1 = digitalRead(SENSORPIN1);
  sensorState2 = digitalRead(SENSORPIN2);
  buttonState = digitalRead(BUTTONPIN);
  if (millis() - lastButtonStateChange >= debounceDuration) {
    if (buttonState != lastButtonState) {
      lastButtonStateChange = millis();
      if (buttonState == LOW) {
        buttonPushCounter++;
      }
      lastButtonState = buttonState;
    }
  }
  if (buttonPushCounter == 1) {
    robotMoveCount = 1;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    buttonPushCounter = 2;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if (((buttonPushCounter == 3) || (robotCycle == true)) && (ledDone == true)
      && (sensorState1 == LOW) && (sensorState2 == HIGH)) {
    robotMoveCount = 2;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    buttonPushCounter = 4;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if (buttonPushCounter == 5) {
    robotMoveCount = 3;
    robotEnable = true;
    ledDone = false;
    blinkState = LOW;
    robotCycle = false;
    buttonPushCounter = 6;//the led state wont change if the button counter is the same
    robotWarningTime = currentMillis;
  }
  if ((currentMillis - robotWarningTime >= 1000) && (ledDone == false)) {
    blinkState = HIGH;
    event = false;
    digitalWrite(LEDPIN, HIGH);
    robotWarning = true;
  }
  if (robotEnable == false) {
    digitalWrite(LEDPIN, LOW);
    event = true;
    ledDone = true;
    robotWarning = false;
  }
  if ((buttonPushCounter == 4) && (robotMoveCount == 1)){
    buttonPushCounter = 5;
  }
}

void led_blink() {

  if (event == true) {
    if (blinkState == LOW) {
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        if (ledState == LOW) {
          ledState = HIGH;

        } else {
          ledState = LOW;
        }
        digitalWrite(LEDPIN, ledState);
      }
    }
  }
}

void robot() {
  if ((robotMoveCount == 1) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85);
    delay(30);
    servo2.write(140);
    delay(250);
    servo3.write(120);
    delay(250);
    servo4.write(100);
    delay(250);
    servo5.write(110);
    robotEnable = false;
  }
  if ((robotMoveCount == 2) && (robotEnable == true) && (robotWarning == true)) {
    if (robotMove == true) {
      servo4.write(40);
      delay(250);
      servo5.write(50);
      delay(250);
      servo4.write(120);
      delay(250);
      servo5.write(90);
      delay(250);
      servo4.write(100);
      time1 = currentMillis;
      robotMove = false;
      robotMoveDone = 1;
    }
    if (robotMoveDone == 1) {
      if (currentMillis - time1 >= 2000) {
        servo1.write(50);
        delay(250);
        servo2.write(130);
        delay(250);
        servo3.write(60);
        delay(250);
        servo5.write(150);
        time2 = currentMillis;
        robotMoveDone = 2;
      }
    }
    if (robotMoveDone == 2) {
      if (currentMillis - time2 >= 2000) {
        servo1.write(85);
        delay(250);
        servo2.write(160);
        delay(250);
        servo3.write(170);
        delay(250);
        servo5.write(90);
        robotMoveDone = 0;
        robotMove = true;
        robotEnable = false;
        robotCycle = true;
      }
    }
  }
  if ((robotMoveCount == 3) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85);
    servo2.write(160);
    delay(250);
    servo3.write(180);
    delay(250);
    servo4.write(100);
    servo5.write(150);
    buttonPushCounter = 0;
    robotMoveCount = 0;
    robotEnable = false;
  }
}