//--------------------------------------------------------------
//-- Move the servos using the test button. When it is pressed
//-- the servos increases their positions (all the servos the same)
//-- When the limits are reached, the movement is performed in the
//-- oposite direction
//--------------------------------------------------------------
//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
//-- GPL license
//--------------------------------------------------------------

#include <Servo.h> 

//-- Mapping between the servo name (in the skymega board) and the
//-- arduino pins
const int SERVO2 = 8;
const int SERVO4 = 9;
const int SERVO6 = 10;
const int SERVO8 = 11;

//-- Array for accesing the 4 servos
Servo myservo[4];  

//-- Skymega's test button is attached to pin 12
//--   It has no pull-up resistor
const int BUTTON = 12;
//-- Button states
const int PRESSED = LOW;
const int NOT_PRESSED = HIGH;
 
void setup() 
{
  //-- Attaching the 4 servos
  myservo[0].attach(SERVO2);
  myservo[1].attach(SERVO4);
  myservo[2].attach(SERVO6);
  myservo[3].attach(SERVO8);
  
  //-- The button is an input
  pinMode(BUTTON, INPUT);
  //-- Activate the button pull-up resistor
  digitalWrite(BUTTON, HIGH); 
}  

//-- Current servo pos
int pos=0;

//-- For reading the test button
int button;

//-- Servo angle increment
int inc=1;

//-- Absolute value of the max position
const int MAX_ANG = 80;


void loop() 
{
  //-- Update the servoS pos
  for (int i=0; i<4; i++)
    myservo[i].write(pos+90);
  
  // read the button
  button = digitalRead(BUTTON); 
  
  //-- If the button is pressed increase the servos pos
  if (button==PRESSED) {
    
    //-- Update the position
    pos+=inc;
    
    //-- If superior limit reach, move in the opposite direction
    if (inc>0 && pos>=MAX_ANG) {
      pos=MAX_ANG;
      inc=-inc;
    }
    
    //-- If the inferior limit is reached, move in the opposite direction
    if (inc<0 && pos<=-MAX_ANG) {
      pos=-MAX_ANG;
      inc=-inc;
    }
    delay(20);
  }
}