#include <ESP32Servo.h>


struct servoType {
   int actPos;
   int stepUp;
   int stepDown;
   unsigned long lastMove;
   unsigned long waitTime;
   boolean GoUp;
   Servo servo;
};
 
void Move(servoType &s, int targetPos){
  unsigned long currentMillis = millis();
   if (currentMillis - s.lastMove > s.waitTime){
      if (s.actPos < targetPos){
          s.lastMove = currentMillis;
          s.actPos += s.stepUp;
          if (s.actPos >= targetPos){
            s.actPos = targetPos; 
            s.GoUp = false;
          } 
          s.servo.write(s.actPos);
      } else 
        if (s.actPos > targetPos){
          s.lastMove = currentMillis;
          s.actPos -= s.stepDown;
          if (s.actPos <= targetPos){ 
            s.actPos = targetPos;
            s.GoUp = true;
           }
          s.servo.write(s.actPos);
        }
   }
}


enum States {WAIT_UP, WAIT_DW,GO_UP, GO_DOWN};

const int UpPos   = 90;
const int DownPos =  0;


servoType myservo1 = {0,2,10,0,45, true};       // 1. Servo
servoType myservo2 = {0,2,10,0,45, true};;       // 2. Servo
int laser = 2;    // AZS 2
int state = GO_UP;

void StateMachine(){
  static unsigned long StartWait = 0;
  static int WaitTime = 0;
   switch(state) {
     case WAIT_UP : if(millis()-StartWait > WaitTime) state = GO_UP;
                    break;
     case WAIT_DW : if(millis()-StartWait > WaitTime) state = GO_DOWN;
                    break;
     case GO_UP   : Move(myservo1,UpPos);
                    Move(myservo2,UpPos);
                    if (myservo2.actPos >= UpPos) {
                        state = WAIT_DW;
                        WaitTime = 1000;
                        StartWait = millis();
                    }    
                    break;
     case GO_DOWN : Move(myservo1,DownPos);
                    Move(myservo2,DownPos);
                    if (myservo2.actPos <= DownPos) {
                        state = WAIT_UP;
                        WaitTime = 2000;
                        StartWait = millis();
                    }
                    break;
     default      : break;               
   }

}


void setup() 
{ 
  myservo1.servo.attach(12);            // Servo1 an pin12
  myservo2.servo.attach(15);            //Servo2 an pin15
  myservo1.servo.write(0);
  myservo2.servo.write(0);
  state = GO_UP;
  pinMode(laser, OUTPUT);  
  delay(500);
  digitalWrite(laser, HIGH);
} 
 

void loop() 
{ 
  StateMachine();
}