//--------------------------------------------------------------
//-- Playing a sequence in the 4 servos
//-- The servo positions are defined in a table. After some time
//-- the servos are updated to their next positions. Also, if the
//-- test button is clicked, the servos move the their next positions
//--------------------------------------------------------------
//-- (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); 
  
  Serial.begin(9600);
}  

//-- This function returns true when the test button
//-- is pressed
bool button_clicked()
{
  
  static int button=1;
  static int oldbutton=1;
  
  //-- Save the previous button state
  oldbutton=button;
  
  // read the button
  button = digitalRead(BUTTON);

  //-- Detect a change in the button
  if (button!=oldbutton) {
    delay(20); //-- for debouncing
    if (button==PRESSED)
      return true;
  } 
  
  return false;
}

//-- This function returns true when the time "time"
//-- has passed since the last call
bool timeout(long time)
{
  static long previousMillis = 0; 
  static long currentMillis;
  
  //-- Read current time
  currentMillis = millis();
 
  //-- Check if the timeout has passed
  if(currentMillis - previousMillis > time) {
    previousMillis = currentMillis;   

    return true;
  }
  
  return false;
}



//-- Matrix of positions. 
//-- There are one column per servo
//-- The rows determine the position of the 4
//-- servos. The controller will read one row,
//-- set the servo positions, wait and go to the
//-- next row
//-- The user can change the values of the table. Can be added
//-- as many rows as desired
int pos[][4] = {
  
  //S1   S2     S3      S4    //-- Servos
  {0,     45,   -45,     0},  //-- Initial pos
  {-45,   0,     45,     0},  //-- Pos 1
  {45,    -45,   0,      0},  //-- Pos2
  //-- More rows can be added...
};


//-- Calculate the number of rows
int size = sizeof(pos)/(4*sizeof(int));

//-- The row that is being read
int i=0;

//-- Time in ms waiting before moving to the next position
const long TIME = 3000;

void update_servos()
{
    //-- Position the 4 servos to the their corresponding
    //-- angle according to the sequence
    for (int s=0; s<4; s++) {
      //-- Position servo s at the pos i
      myservo[s].write(pos[i][s]+90);
    } 
}

void loop() 
{ 
  //-- If have passed TIME ms or the test button has been clicked...
  if (timeout(TIME) || button_clicked()) {
  
    //-- Update the servos to their new positions
    update_servos();
    
    //-- change to the next position
    i=(i+1) % size;
  }  
}