#include <Servo.h> // Include servo library
#define STAY_STILL 1500

void general_rotation(bool clockwise,double w,double radius,double constant,unsigned int delay_time);
Servo servoLeft; // Declare left servo signal
Servo servoRight; // Declare right servo signal

void setup() // Built in initialization block
{
    unsigned int delay_time;
    double speed,w,radius;
    bool right;
    // arbitrary values for the speed computation of the servos given a radius
    const double constant=4.3;// inches
    w=100;//
    servoLeft.attach(13); // Attach left signal to pin 13
    servoRight.attach(12); // Attach right signal to pin 12
    //You can change these values, if you are brave enough...
    delay_time=10000;
    radius=13; // Select a value between 3 and 30
    right=false;
    general_rotation(right,w,radius,constant,delay_time);
    servoLeft.detach();                        // Stop sending servo signals
    servoRight.detach();
}

void loop() // Main loop auto-repeats
{ // Empty, nothing needs repeating
}

//Radius must be specified
void general_rotation(bool clockwise,double w,double radius,double constant,unsigned int delay_time)
{
    if(radius<3) radius=3;
    else if(radius>30) radius=30;
    int speed1=static_cast<int>((w*(radius+constant/2))*100/3215);
    int speed2=static_cast<int>((w*(radius-constant/2))*100/3215);  
    if(clockwise)
    {
      servoLeft.writeMicroseconds(STAY_STILL+speed1);
      servoRight.writeMicroseconds(STAY_STILL-speed2);
    }
    else
    {
      servoLeft.writeMicroseconds(STAY_STILL+speed2);
      servoRight.writeMicroseconds(STAY_STILL-speed1);
    }
    delay(delay_time);
    return;
}