#include <Servo.h>

// Create servo objects for each servo
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

const int PLCOUT_100_00 = 4;
const int PLCOUT_100_01 = 7;
const int PLCOUT_100_02 = 8;

bool readPLC100_00();
bool readPLC100_01();
bool readPLC100_02();
void module2servos(bool signal);
void module3servosA(bool signal);
void module3servosB(bool signal);

void setup() {
  // put your setup code here, to run once:
    // Attach the servos to their respective pins
  servo1.attach(3); //module 2
  servo2.attach(5); //module 2
  servo3.attach(6); //module 3 A1
  servo4.attach(9); //module 3 A2
  servo5.attach(10); //module 3 B1
  servo6.attach(11); //module 3 B2

  pinMode(PLCOUT_100_00, INPUT_PULLUP);
  pinMode(PLCOUT_100_01, INPUT_PULLUP);
  pinMode(PLCOUT_100_02, INPUT_PULLUP);

}

void loop() {

  bool signalPLCOUT100_00 = readPLC100_00();
  bool signalPLCOUT100_01 = readPLC100_01();
  bool signalPLCOUT100_02 = readPLC100_02();
  module2servos(signalPLCOUT100_00);
  module3servosA(signalPLCOUT100_01);
  module3servosB(signalPLCOUT100_02);

}


bool readPLC100_00() {
  static unsigned long PLCOUT100_00_millis = millis();
  if (millis() - PLCOUT100_00_millis > 50) {
    bool signal = digitalRead(PLCOUT_100_00);
    return signal;
    PLCOUT100_00_millis = millis();
  }
}

bool readPLC100_01() {
  static unsigned long PLCOUT100_01_millis = millis();
  if (millis() - PLCOUT100_01_millis > 50) {
    bool signal = digitalRead(PLCOUT_100_01);
    return signal;
    PLCOUT100_01_millis = millis();
  }
}

bool readPLC100_02() {
  static unsigned long PLCOUT100_02_millis = millis();
  if (millis() - PLCOUT100_02_millis > 50) {
    bool signal = digitalRead(PLCOUT_100_02);
    return signal;
    PLCOUT100_02_millis = millis();
  }
}

void module2servos(bool signal) {
  static unsigned long mod2Servomillis = millis();
  if (millis() - mod2Servomillis > 100) {
    if (signal == LOW){ 
      servo1.write(135);
      servo2.write(135);
    }
    else {
      servo1.write(45);
      servo2.write(45);
    }
  mod2Servomillis = millis();
  }
}

void module3servosA(bool signal) {
  static unsigned long mod3ServosAmillis = millis();
  if (millis() - mod3ServosAmillis > 100) {
    if (signal == LOW){ 
      servo3.write(135);
      servo4.write(135);
    }
    else {
      servo3.write(45);
      servo4.write(45);
    }
  mod3ServosAmillis = millis();
  }
}

void module3servosB(bool signal) {
  static unsigned long mod3ServosBmillis = millis();
  if (millis() - mod3ServosBmillis > 100) {
    if (signal == LOW){ 
      servo5.write(135);
      servo6.write(135);
    }
    else {
      servo5.write(45);
      servo6.write(45);
    }
  mod3ServosBmillis = millis();
  }
}