//--------------------------------------------------------------
//-- Controlling the position of the 4 servos using the Skypads
//-- board
//--------------------------------------------------------------
//-- (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];  

//-- Mapping between the skypads and skymega pins
const int UP_1    = 2;
const int LEFT_1  = 3;
const int DOWN_1  = 4;
const int RIGHT_1 = 5;

const int UP_2   = 6;
const int LEFT_2 = 7;
const int DOWN_2 = A0;
const int RIGHT_2= A1;

//-- Array of buttons. This allows reading the button 
//-- state by reading the array
const int skypads[]={UP_1, DOWN_1, LEFT_1, RIGHT_1,
                     UP_2, DOWN_2, LEFT_2, RIGHT_2};

//-- 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);
  
  //-- Configure all the skypads button as inputs
  //-- No internal pull-up should be activated
  //-- (the skypads already have pull-ups resistors)
  for (int i=0; i<8; i++)
    pinMode(skypads[i],INPUT);
    
  Serial.begin(9600);
  
}  

//-- Current servo pos
int pos[4]={0,0,0,0};

//-- Buttons for increasing/decreasing the servo pos
//-- It can be configured accorting to the user's preferences
//--                        S1       S2       S3      S4
const int button_inc[4] = {UP_1  , RIGHT_1 , UP_2,   RIGHT_2};
const int button_dec[4] = {DOWN_1, LEFT_1,  DOWN_2,  LEFT_2};

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

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


void loop() 
{
  
  //-- Repeat for every servo
  for (int i=0; i<4; i++) {
    
    //-- If increasing button of servo i is pressed
    if (digitalRead(button_inc[i])==PRESSED) {
      
      //-- Increase servo pos if the superior limited has not
      //-- already been reached
      if (pos[i]<MAX_ANG) {
        //-- Update the position
        pos[i]+=inc;
      } 
      else pos[i]=MAX_ANG;
    }
    else
      // If decreasing button if servo i is pressed...
      if (digitalRead(button_dec[i])==PRESSED) {
        
        //-- Decrease the servo pos... if it has not already
        //-- reached the inferior limit
        if (pos[i]>-MAX_ANG) {
          pos[i]-=inc;
        }
        else pos[i]=-MAX_ANG;
      }
    
    myservo[i].write(pos[i]+90);
  }  
  
  //-- The servos will remain at least 20ms in every position
  delay(20);
}