// Servo colab

#include "SoftwareServo.h"

SoftwareServo servoL;
SoftwareServo servoR;

int servoMaxPulse;

void setup()
{
  servoL.attach(3);
  servoL.setMaximumPulse(2580);
  servoR.attach(2);
  servoR.setMaximumPulse(2633);
  
  Serial.begin(19200);
          
  Serial.println("Start! ");
}

void loop()
{

  delay(1000);
  
  Serial.println("Rotate Test 90, 2 sec");
  spinServos(2000, 90, 90);
  
  Serial.println("Rotate Test Full Forward, 2 sec");
  spinServos(2000, 0, 180);

  Serial.println("Rotate Test Full Back, 2 sec");
  spinServos(2000, 180, 0);

  Serial.println("Rotate Test Turn Right, 2 sec");
  spinServos(2000, 180, 180);

  Serial.println("Rotate Test Turn Left, 2 sec");
  spinServos(2000, 0, 0);

  Serial.println("Wait, 5 sec");
  delay(5000);
}

void spinServos(int ForMills, int LRotation, int RRotation) {
  int i = 0;
  
  for(i = 0; i < ForMills; i += 1){
    servoR.write(RRotation);
    servoL.write(LRotation);
    
    SoftwareServo::refresh();
    delay(1);
  }
}