#include <Arduino.h>

#include "ServoEasing.hpp"


ServoEasing ArmLeftCrew;
ServoEasing ArmRightCrew;
ServoEasing HeadLRCrew;
ServoEasing HeadHLCrew;
ServoEasing BodyLRCrew;
ServoEasing BodyHLCrew;


int startALC = 0;
int startARC = 0;
int HLRC = 0;
int HHLC = 0;
int BLRC = 0;
int BHLC = 0;



void setup() {
    // Servo1.attach(6, 45); Example 45 is start degree
    ArmLeftCrew.attach(5, 45);
    ArmRightCrew.attach(6, 100);
    HeadLRCrew.attach(7, 45);
    HeadHLCrew.attach(8, 0);
    BodyLRCrew.attach(9, 0);
    BodyHLCrew.attach(10, 0);

    delay(500); // Wait for servo to reach start position.
}

void loop() {
    //Servo1.easeTo(135, 40); it will bock for 40degree per second;
    delay(45);
    ArmLeftCrew.easeTo(135 ,40);
    ArmRightCrew.easeTo(125, 100);
    BodyHLCrew.easeTo(130, 80);
    delay(500);
    ArmRightCrew.easeTo(0, 50);
    delay(450);
    ArmRightCrew.easeTo(35, 25);
    delay(250);
    ArmLeftCrew.easeTo(0, 40);
    delay(350);
    BodyHLCrew.easeTo(0, 40);



    
}