Robot arm with led blink warning before it moves and button

Other Hardware Robotics
4

This code fixed a bug in the original post. The bug was if the button is pressed three times and the break beam sensors had not been triggered the robot arm would not go off, it would take four button pushes. Code in simulator also updated.

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

VarSpeedServo servo1;
VarSpeedServo servo2;
VarSpeedServo servo3;
VarSpeedServo servo4;
VarSpeedServo 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)){//these lines fixed bug that if button is
    buttonPushCounter = 5;                               //pushed three times it turns the robot
  }                                                      //off again if break beam sensors are not
}                                                        //trigged without these lines it would
                                                         //take four button pushes.
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, 70, true);
    servo2.write(140, 70, true);
    servo3.write(120, 70, true);
    servo4.write(100, 70, true);
    servo5.write(110, 70, true);
    robotEnable = false;
  }
  if ((robotMoveCount == 2) && (robotEnable == true) && (robotWarning == true)) {
    if (robotMove == true) {
      servo4.write(40, 70, true);
      servo5.write(50, 70, true);
      servo4.write(120, 70, true);
      servo5.write(90, 70, true);
      servo4.write(100, 70, true);
      time1 = currentMillis;
      robotMove = false;
      robotMoveDone = 1;
    }
    if (robotMoveDone == 1) {
      if (currentMillis - time1 >= 2000) {
        servo1.write(50, 60, true);
        servo2.write(130, 60, true);
        servo3.write(60, 60, true);
        servo5.write(150, 70, true);
        time2 = currentMillis;
        robotMoveDone = 2;
      }
    }
    if (robotMoveDone == 2) {
      if (currentMillis - time2 >= 2000) {
        servo1.write(85, 60, true);
        servo2.write(160, 60, true);
        servo3.write(170, 60, true);
        servo5.write(90, 70, true);
        robotMoveDone = 0;
        robotMove = true;
        robotEnable = false;
        robotCycle = true;
      }
    }
  }
  if ((robotMoveCount == 3) && (robotEnable == true) && (robotWarning == true)) {
    servo1.write(85, 60);
    servo2.write(160, 60, true);
    servo3.write(180, 60, true);
    servo4.write(100, 60);
    servo5.write(150, 60);
    buttonPushCounter = 0;
    robotMoveCount = 0;
    robotEnable = false;
  }
}