#include <Servo.h>
#include <Oscillator.h>

//-- Declare the oscillators
Oscillator osc_middle, osc_right, osc_left;

//-- Global parameters for the oscillators
unsigned int A=40; // Amplitude (higher -> longer steps) set 10-40
unsigned int T=5000; // Period (lower -> faster moves)

void setup()
{
Seiral.begin(9600);
  delay(1000); // Small startup delay, replace with start-button press (if present)
  
  //-- Attach the oscillators to the servos
  osc_middle.attach(2); // middle servo
  osc_right.attach(3);  // right servo
  osc_left.attach(4);   // left servo
  
  //-- Set the parameters
  osc_middle.SetO(-15); // Correction for the offset of the servos
  osc_right.SetO(-20);
  osc_left.SetO(25);
  
  osc_middle.SetA(15); // Middle motor needs a small amplitude (5-10)
  osc_right.SetA(A);
  osc_left.SetA(A);
  
  osc_middle.SetT(T); // Set the period of work
  osc_right.SetT(T);
  osc_left.SetT(T);
  
  //-- Refresh the oscillators
  osc_middle.refresh();
  osc_right.refresh();
  osc_left.refresh();

  //-- Set the phase difference
  //-- This defines the type of movement the robot makes
  osc_middle.SetPh(DEG2RAD( 90 ));
  osc_left.SetPh(  DEG2RAD( 0 )); 
  osc_right.SetPh( DEG2RAD( 0 )); // For example, putting 180 here will make the robot spin clockwise

}

void loop()
{

Char a=Serial.read();
  
  if(a==‘1’) 

{ // Forward
    osc_right.SetPh( DEG2RAD( 0 ));
    osc_left.SetPh( DEG2RAD( 0 ));
    
  } 

else if(a==‘2’) 
{ // Counter clockwise
    osc_right.SetPh( DEG2RAD( 0 ));
    osc_left.SetPh( DEG2RAD( 180 ));
    
  } 

else if(a==‘3’) 

{ // Backward
    osc_right.SetPh( DEG2RAD( 180 ));
    osc_left.SetPh( DEG2RAD( 180 ));
    
  } 

else if(a==‘4’) 
{ // Counter clockwise
    osc_right.SetPh( DEG2RAD( 180));
    osc_left.SetPh( DEG2RAD( 0 ));
    
  } 

else if(a==‘5’) 

{ // Idle position
    osc_right.SetPh( DEG2RAD( 90 ));
    osc_left.SetPh( DEG2RAD( 90 ));
    
  } }