//Articulated arm - experiment and adjust to suit your circuit 

#include <Servo.h>
Servo servo1, servo2, servo3;

struct Setpoint{
  int servo1, servo2, servo3;
};

Setpoint setpoints[100];
int setpointCount=0;

void move(Setpoint start, Setpoint end){
  //Slowly move the servos from a start to an end point
  int numSteps=100, milliseconds=1000;
  for(int i=0; i<100; i++){
    servo1.write(int(start.servo1+(end.servo1-start.servo1)*i/numSteps));
    servo2.write(int(start.servo2+(end.servo2-start.servo2)*i/numSteps));
    servo3.write(int(start.servo3+(end.servo3-start.servo3)*i/numSteps));
    delay(milliseconds/numSteps);
  }
}

struct Setpoint potentiometerPoint(){
  //Returns current potentiometer point
    Setpoint point;
    long int potentiometer1=analogRead(A1);
    point.servo1=int(potentiometer1*180/1024);
    long int potentiometer2=analogRead(A2);
    point.servo2=int(potentiometer2*180/1024);
    long int potentiometer3=analogRead(A3);
    point.servo3=int(potentiometer3*180/1024);
    return point;
}

void setup()
{
  //Configure the potentiometer pins for ADC input
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  
  //attach each servo to a PWM-capable pin
  //the servo library sets the 50Hz pulse frequency
  servo1.attach(5, 1000, 2000);
  servo2.attach(6, 1000, 2000);
  servo3.attach(4, 1000, 2000);
  
  //Configure pushbuttons for digital input with pullup resistors
  pinMode(10,INPUT_PULLUP);
  pinMode(11,INPUT_PULLUP);
  pinMode(12,INPUT_PULLUP);
  
  Serial.begin(9600);
  
  reset();
}

void loop(){
  long int potentiometer1=analogRead(A1);
  long int potentiometer2=analogRead(A2);
  long int potentiometer3=analogRead(A3);
  if(digitalRead(10)==0){		//add a new setpoint
    if(setpointCount<100){
      beep(1);
      setpoints[setpointCount]=potentiometerPoint();
      setpointCount=setpointCount+1;
    } else beep(2); //no more set points available
  }else if(digitalRead(11)==0){	//delete the last setpoint
    if(setpointCount>0){
      beep(1);
      setpointCount=setpointCount-1;
    } else beep(2); //no setpoints to delete
  }else if(digitalRead(12)==0){	//play the recorded setpoints
    if(setpointCount>0){
      beep(1);
      Setpoint start=potentiometerPoint();
      for(int i=0; i<setpointCount; i=i+1){
        move(start, setpoints[i]);
        start=setpoints[i];
        delay(500);
      }
      move(start, potentiometerPoint());
      beep(2);      //finished playing back setpoints
    } else beep(2); //no setpoints to play back
  }else{						//control servos with the potentiometers
    servo1.write(int(potentiometer1*180/1024));
    servo2.write(int(potentiometer2*180/1024));
    servo3.write(int(potentiometer3*180/1024));
  }
  
  delay(100);
}

void reset(){
  //Adjust servos one at a time to limit power draw
  long int potentiometer1=analogRead(A1);
  servo1.write(int(potentiometer1*180/1024));
  delay(500);
  long int potentiometer2=analogRead(A2);
  servo2.write(int(potentiometer2*180/1024));
  delay(500);
  long int potentiometer3=analogRead(A3);
  servo3.write(int(potentiometer3*180/1024));
  delay(500);
  beep(2);  //system ready for use
}

void beep(int numBeeps){
  //Turn the buzzer on and off a given number of times
  for(int i=0; i<numBeeps; i=i+1){
    tone(9,1000);
    delay(300);
    noTone(9);
    delay(200);
  }
}