// https://forum.arduino.cc/t/ein-2-servo-problem/1352398

#include <Servo.h>

Servo myservo_1;
Servo myservo_2;

constexpr byte PIN_PHOTO_SENSOR  {A5};        // Eingang Fotowiderstand
constexpr byte PIN_SERVO_1         {3};         // Servo PWM D3
constexpr byte PIN_SERVO_2         {5};         // Servo PWM D5
constexpr byte PIN_LED           {13};        // Platinen LED

constexpr uint16_t INPUT_VOLTAGE_RELEASE     {400};   // Haltewiderstand
constexpr uint16_t INPUT_VOLTAGE_HOLD        {100};    // Freigabewiderstand

constexpr uint16_t SERVO_1_POSITION_RELEASE      {150};    // Servo Rechts Armstellung für halten und Freigabe
constexpr uint16_t SERVO_1_POSITION_HOLD         {90};
constexpr uint16_t SERVO_2_POSITION_RELEASE      {30};    // Servo Links Armstellung für halten und Freigabe
constexpr uint16_t SERVO_2_POSITION_HOLD         {90};


uint16_t photosensor_voltage;


byte switchStep;
constexpr bool an {HIGH};
constexpr bool aus {!an};

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

  pinMode(PIN_PHOTO_SENSOR, INPUT);
  pinMode(PIN_LED, OUTPUT);
  myservo_1.attach(PIN_SERVO_1);
  myservo_2.attach(PIN_SERVO_2);
  // definierte Servopositionen
  digitalWrite(PIN_LED, aus);
  myservo_1.write(SERVO_1_POSITION_HOLD);
  myservo_2.write(SERVO_2_POSITION_HOLD);
}

void loop()
{
  photosensor_voltage = analogRead(PIN_PHOTO_SENSOR);
  Serial.println(photosensor_voltage);
  if (digitalRead(PIN_LED) == aus)
  {
    if (photosensor_voltage < INPUT_VOLTAGE_RELEASE)
    {
      digitalWrite(PIN_LED, an);
    }
  }
  else if (photosensor_voltage >= INPUT_VOLTAGE_HOLD)
  {
    digitalWrite(PIN_LED, aus);
    switchStep++;
  }
  switch (switchStep)
  {
    case 1:
      myservo_1.write(SERVO_1_POSITION_RELEASE);
      break;
    case 2:
      myservo_1.write(SERVO_1_POSITION_HOLD);
      break;
    case 3:
      myservo_2.write(SERVO_2_POSITION_RELEASE);
      break;
    case 4:
      myservo_2.write(SERVO_2_POSITION_HOLD);
      break;
    default:
      switchStep = 0;
      break;
  }

  delay(500);
}