//--------------------------------------------------------------
//-- Oscillator4.pde
//-- Produce oscillations in 4 servos with the same Amplitud (A),
//-- offset(O), period(T) and a fixed phase difference (PD) between
//-- two consecutive oscillators.
//-- It is the base for generating locomotion of modular robots
//--------------------------------------------------------------
//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
//-- GPL license
//--------------------------------------------------------------
#include <Servo.h>
#include <Oscillator.h>
#include "skymega.h"

//-- Macro for converting from degrees to radians
#define DEG2RAD(g) ((g)*M_PI)/180

//-- Movement parameters
//-- USER: Change these parameters acording to what wants 
//--   to be generated
const unsigned int T=1500;    //-- Period (ms)
const unsigned int A=40;      //-- Amplitude (deg)
const int O=30;               //-- Offset (deg)
const int PD=-45;    //-- Phase difference (deg)
const int PHASE0=0;  //-- Initial phase (deg)

//-- Build a robot with the following servos. 
//-- where robot_servo[0] is the servo in the tail and 
//-- robot_servo[3] the head
//-- It is not necesary to specify 4 servos. The size will
//-- be dynamically calculated (it can be 1, 2, 3...)
int robot_servo[]={SERVO2, SERVO4, SERVO6, SERVO8};

//-- Number of servos in the robot
//-- It is calculated from robot_servo[]
const int NSERVOS = sizeof(robot_servo)/sizeof(int);

//-- Use 4 oscillators
Oscillator osc[4];

void setup()
{
  //-- Attach the oscillators to the servos
  //-- and set the oscillators parameters
  for (int i=0; i<NSERVOS; i++) {
    osc[i].attach(robot_servo[i]);
    osc[i].SetA(A);
    osc[i].SetO(O);
    osc[i].SetT(T);
    osc[i].SetPh(DEG2RAD(PHASE0 + i*PD));
  }
}

void loop()
{
  //-- Refresh the oscillators
  for (int i=0; i<NSERVOS; i++)
    osc[i].refresh();
}