//On standard servos a parameter value of 1000 is fully counter-clockwise, 2000 is fully clockwise, and 1500 is in the middle.

#include <Servo.h>

Servo servobase;          // Define left servo
Servo servoshoulder;         // Define right servo
Servo servoforearm;

void setup() { 
  servobase.attach(9, 1000, 2000);  // Set left servo to digital pin 10
  servoshoulder.attach(10, 1000, 2000);  // Set right servo to digital pin 9
  servoforearm.attach(11, 1000, 2000); // center is 1500, Left is 2000, right is 1000 (Most common servos accept 
  //inputs from 1000 µs to 2000 µs, with 1500 µs corresponding to the center position. For a 0-180° servo, this would be 90°.
} 

void loop() {            // Loop through motion tests
  home1();     // Example: move forward           // Wait 2000 milliseconds (2 seconds)
  delay(1000);
  pos1();
  delay(800);
  pos2();
  delay(800);
  pos3();
  delay(800);
  pos4();
  delay(800);
  pos5();
  delay(800);
  pos6();
  delay(800);
  pos7();
  delay(800);
  pos8();
  delay(8000);
}

// Motion routines for forward, reverse, turns, and stop
//1600 is downward on both shoulder and forearm
void home1() {
  servobase.write(90);
  servoshoulder.write(50);
  servoforearm.write(180);
  
}

void pos1() {
  servobase.write(90);
  servoshoulder.write(42);
  servoforearm.write(180);
  
}

void pos2() {
  servobase.write(90);
  servoshoulder.write(42);
  servoforearm.write(177);
  
}

void pos3() {
  servobase.write(90);
  servoshoulder.write(42);
  servoforearm.write(174);
  
}

void pos4() {
  servobase.write(90);
  servoshoulder.write(41);
  servoforearm.write(171);
  
}

void pos5() {
  servobase.write(90);
  servoshoulder.write(41);
  servoforearm.write(168);
  
}

void pos6() {
  servobase.write(90);
  servoshoulder.write(41);
  servoforearm.write(165);
  
}

void pos7() {
  servobase.write(90);
  servoshoulder.write(40);
  servoforearm.write(162);
  
}

void pos8() {
  servobase.write(90);
  servoshoulder.write(40);
  servoforearm.write(159);
  
}

 
