#include "ServoEasing.h" //https://www.airspayce.com/mikem/arduino/AccelStepper/classAccelStepper.html

//****** PINS ******//
#define CENTER_SERVO     5
#define ELBOW_SERVO      6
#define DISPENSE_SERVO   9


//***** SETUPS *****//
#define load_position   114
#define drop_position   95

#define center_home     95
#define elbow_home      57

ServoEasing Center;
ServoEasing Elbow;
ServoEasing Dispenser;

void setup() {

  //setup Serial
  Serial.begin(9600);
  Serial.println("SCARA style robot arm demo program");
  delay(500);

  Center.attach(CENTER_SERVO);
  Elbow.attach(ELBOW_SERVO);
  Dispenser.attach(DISPENSE_SERVO);

  //Set location to home

  Dispenser.write(drop_position);
  Center.write(center_home);
  Elbow.write(elbow_home);

  setSpeedForAllServos(30);

  delay(1000);

  //Setup movements begin here

  Elbow.setEasingType(EASE_CUBIC_IN_OUT);
  Center.setEasingType(EASE_CUBIC_IN_OUT);

  //Move arm inward

  sServoNextPositionArray[0] = center_home-40;
  sServoNextPositionArray[1] = elbow_home+80;

  setEaseToForAllServosSynchronizeAndStartInterrupt(90);

  delay(1000);

  dispense(); //Drop a bearing

  //Move arm back to start position

  sServoNextPositionArray[0] = center_home;
  sServoNextPositionArray[1] = elbow_home;

  setEaseToForAllServosSynchronizeAndStartInterrupt(90);
   
}

void dispense(){

  Dispenser.write(load_position);
  delay(100);
  Dispenser.write(drop_position);
  delay(500);
  
}

void loop() {

}