FSM with Servos

#include <Servo.h>
#include "YA_FSM.h"     // https://github.com/cotestatnt/YA_FSM

#define SERVO1_PIN      9
#define SERVO2_PIN      10

// Target servos position
unsigned int servo1_Target, servo2_Target;

// Servos state transitions
bool servo1_Tr1, servo1_Tr2;
bool servo2_Tr1, servo2_Tr2;

// States Aliases
enum servo1_State {servo1_State1, servo1_State2};
enum servo2_State {servo2_State1, servo2_State2};

// Create new FSM for servo1 (num States, num Transition)
YA_FSM servo1_FSM(2, 2);

// Create new FSM for servo2 (num States, num Transition)
YA_FSM servo2_FSM(2, 2);

// Create the servo objects
Servo servo1, servo2;


void setupStateMachine(){
    // Add state transitions
    servo1_FSM.AddTransition(servo1_State1, servo1_State2, servo1_Tr1 );
    servo1_FSM.AddTransition(servo1_State2, servo1_State1, servo1_Tr2 );

    // Add on leaving actions for each state
    servo1_FSM.SetOnLeaving(servo1_State1, [](){servo1_Target = 30;} );
    servo1_FSM.SetOnLeaving(servo1_State2, [](){servo1_Target = 70;} );

    // The two servos states has timeout and do nothing (lambda function => [](){})
    servo1_FSM.SetOnState(servo1_State1, [](){}, 6000 );
    servo1_FSM.SetOnState(servo1_State2, [](){}, 3000 );

    // Add transitions
    servo2_FSM.AddTransition(servo2_State1, servo2_State2, servo2_Tr1 );
    servo2_FSM.AddTransition(servo2_State2, servo2_State1, servo2_Tr2 );

    // Add on leaving actions for each state
    servo2_FSM.SetOnLeaving(servo2_State1, [](){servo2_Target = 30;} );
    servo2_FSM.SetOnLeaving(servo2_State2, [](){servo2_Target = 70;} );

    // The two servos states has timeout and do nothing (lambda function => [](){})
    servo2_FSM.SetOnState(servo2_State1, [](){}, 7000 );
    servo2_FSM.SetOnState(servo2_State2, [](){}, 1000 );
}





void setup(){
    Serial.begin(9600);
    Serial.println(F("Starting State Machine...\n"));

    setupStateMachine();

    servo1.attach(SERVO1_PIN);
    servo1_Target = 70;
    servo1.write(servo1_Target);

    servo2.attach(SERVO2_PIN);
    servo2_Target = 70;
    servo2.write(servo2_Target);
}

void loop(){
    // Update FSM for servo1
    if( servo1_FSM.Update()){
        Serial.print(millis());
        Serial.print(" - Servo1 pos: ");
        Serial.println(servo1_Target);
    }
    servo1_Tr1 = servo1_FSM.GetTimeout(servo1_State1);
    servo1_Tr2 = servo1_FSM.GetTimeout(servo1_State2);

    // Write position
    servo1.write(servo1_Target);

    // Update FSM for servo2
    if(servo2_FSM.Update()){
        Serial.print(millis());
        Serial.print(" - Servo2 pos: ");
        Serial.println(servo2_Target);
    }
    servo2_Tr1 = servo2_FSM.GetTimeout(servo2_State1);
    servo2_Tr2 = servo2_FSM.GetTimeout(servo2_State2);

    // Write position
    servo2.write(servo2_Target);

}