// Controlling multiple servo motors with the Arduino Uno.

// Version 1:   https://wokwi.com/projects/369705616834606081
// Version 2:   https://wokwi.com/projects/370198618373197825
// Version 3:   https://wokwi.com/projects/370260137667236865
// Version 4:   https://wokwi.com/projects/370269233463345153  Added separate callback functions for each button.
// Version 4.1: https://wokwi.com/projects/370440521607948289  Added buttons and servos for turntables.
// Version 5:   https://wokwi.com/projects/370269233463345153  Modified turnLeft() and turnRight() to remember the current turn.

#include <slowServo.h>
#include <mechButton.h>

#define TURN_1 180
#define TURN_2 0

#define CLOSE_1 30
#define CLOSE_2 150
#define OPEN_1 90
#define OPEN_2 90
#define MS_DEG 30

mechButton  turntableLeftBtn(13);
mechButton  turntableRightBtn(10);
slowServo   turntableLeftServo(12);
slowServo   turntableRightServo(11);

mechButton  closeLockABtn(2);
mechButton  openLockABtn(3);
slowServo   lockA1Servo(5);
slowServo   lockA2Servo(4);

mechButton  closeLockBBtn(9);
mechButton  openLockBBtn(8);
slowServo   lockB1Servo(6);
slowServo   lockB2Servo(7);

void turnServo(slowServo& servo, uint8_t turn)
{
  int deg = turn == 1 ? TURN_1 : TURN_2;
  servo.setDeg(deg);
}

void turnLeft()
{
  if (!turntableLeftBtn.getState())
  {
    static uint8_t turn = 1;
    turn = turn == 1 ? 2 : 1;
    turnServo(turntableLeftServo, turn);
  }
}

void turnRight()
{
  if (!turntableRightBtn.getState())
  {
    static uint8_t turn = 1;
    turn = turn == 1 ? 2 : 1;
    turnServo(turntableRightServo, turn);
  }
}

void closeLockA()
{
  if (!closeLockABtn.getState())
  {
    lockA1Servo.setDeg(CLOSE_1);
    lockA2Servo.setDeg(CLOSE_2);
  }
}

void closeLockB()
{
  if (!closeLockBBtn.getState())
  {
    lockB1Servo.setDeg(CLOSE_1);
    lockB2Servo.setDeg(CLOSE_2);
  }
}

void openLockA()
{
  if (!openLockABtn.getState())
  {
    lockA1Servo.setDeg(OPEN_1);
    lockA2Servo.setDeg(OPEN_2);
  }
}

void openLockB()
{
  if (!openLockBBtn.getState())
  {
    lockB1Servo.setDeg(OPEN_1);
    lockB2Servo.setDeg(OPEN_2);
  }
}

void setup()
{
  Serial.begin(115200);

  turntableLeftBtn.setCallback(turnLeft);
  turntableRightBtn.setCallback(turnRight);
  turntableLeftServo.begin();
  turntableRightServo.begin();
  turntableLeftServo.setDeg(TURN_1);
  turntableRightServo.setDeg(TURN_1);
  sleep(1000);
  turntableLeftServo.setMsPerDeg(MS_DEG);
  turntableRightServo.setMsPerDeg(MS_DEG);

  closeLockABtn.setCallback(closeLockA);
  openLockABtn.setCallback(openLockA);
  lockA1Servo.begin();
  lockA2Servo.begin();
  lockA1Servo.setDeg(OPEN_1);
  lockA2Servo.setDeg(OPEN_2);
  sleep(1000);
  lockA1Servo.setMsPerDeg(MS_DEG);
  lockA2Servo.setMsPerDeg(MS_DEG);

  closeLockBBtn.setCallback(closeLockB);
  openLockBBtn.setCallback(openLockB);
  lockB1Servo.begin();
  lockB2Servo.begin();
  lockB1Servo.setDeg(CLOSE_1);
  lockB2Servo.setDeg(CLOSE_2);
  sleep(1000);
  lockB1Servo.setMsPerDeg(MS_DEG);
  lockB2Servo.setMsPerDeg(MS_DEG);
}

void loop()
{
  idle();
}