//--------------------------------------------------------------
//-- 
//--------------------------------------------------------------
//-- (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];  
 
void setup() 
{ 
  //-- Attaching the 4 servos
  myservo[0].attach(SERVO2);
  myservo[1].attach(SERVO4);
  myservo[2].attach(SERVO6);
  myservo[3].attach(SERVO8);
  
  Serial.begin(9600);
} 
 
//-- Conversion from degrees to radiands
inline int DEG2RAD(int g)
{
  return g*M_PI/180;
}
 

//---- USER parameters -------------------------

//-- Oscillation period (in milliseconds)
const unsigned int T = 2000;

//-- Oscillator's amplitude
const int A=30;

//-- Oscilltator's offset
const int O=0;

//---------------------------------------------


//-- Internal parameters-----------

//-- Sampling period in miliseconds
const unsigned int TS=20;

//-- Initial phase;
const double phase0=0;
 
//-- Number of samples per period
const unsigned int N = T/TS; 

//-- Phase's angular increment 
const double inc = 2*M_PI/N; 

//-- Internal variables---------------

//-- The phase
double phase=0;
 
//-- Servo pos
int pos; 
 
 
void loop() 
{ 

   //-- There are N samples. The sine function 
   //-- is sampled N times per period
   for (int n=0; n<N; n++) {
     
     //-- Sample the sine function and set the servo pos
     pos = round(A * sin(phase + phase0) + O);
     myservo[0].write(pos+90);

     //-- Increment the phase
     phase = phase + inc;
  
     //-- The samples are taken every TS milliseconds     
     delay(TS);
   }
}