/* Sweep
  by BARRAGAN <http://barraganstudio.com>
  This example code is in the public domain.

  modified 28 May 2015
  by Michael C. Miller
  modified 8 Nov 2013
  by Scott Fitzgerald

  http://arduino.cc/en/Tutorial/Sweep
*/

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
// twelve servo objects can be created on most boards


void setup() {
  Serial.begin(9600);
  myservo.attach(26,700,2500);  // attaches the servo on GIO2 to the servo object
  myservo2.attach(27,700,2500);
  myservo3.attach(28,700,2500);
  myservo4.attach(29,700,2500);
  myservo5.attach(6,700,2500);  
}
// #define DEFAULT_MIN_PULSE_WIDTH 700
// #define DEFAULT_MAX_PULSE_WIDTH 2500
// #define DEFAULT_NEUTRAL_PULSE_WIDTH 1600
// void rock_to_paper(){
//   int pos;
//   myservo5.write(180);
//   delay(100);
//   for (pos = 0; pos <= 180; pos += 1) {
//     myservo.write(pos);
//     myservo2.write(pos);
//     myservo3.write(180-pos);
//     myservo4.write(180-pos);
//     delay(5);
//   }
//   delay(200);
//   // thumb
//   for (pos = 0; pos <= 180; pos += 1) {
//     myservo5.write(180-pos);
//     delay(5);
//   }  
// }

// void paper_to_scissors(){
//   int pos;
//   for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
//     // in steps of 1 degree
//     myservo5.write(pos);
//     delay(5);
//   }
//   delay(200);
//   for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
//     myservo3.write(180-pos);
//     myservo4.write(180-pos);
//     delay(5);                       // waits 15ms for the servo to reach the position
//   }
//   delay(200);
// }

void loop() {
  int pos;

  // spot = myservo.read();
  // Serial.println(spot);
  myservo5.write(180);
  delay(100);
  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    myservo2.write(pos);
    myservo3.write(180-pos);
    myservo4.write(180-pos);
    delay(5);                       // waits 15ms for the servo to reach the position
  }
  // spot = myservo.read();
  // Serial.println(spot);
  delay(200);

  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo5.write(180-pos);
    delay(5);                       // waits 15ms for the servo to reach the position
  }  
  delay(100);
  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo5.write(pos);
    delay(5);                       // waits 15ms for the servo to reach the position
  }  

  delay(200);
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    myservo2.write(pos);
    myservo3.write(180-pos);
    myservo4.write(180-pos);
    delay(5);                       // waits 15ms for the servo to reach the position
  }
  delay(200);
}