// Servo software servo centering script
/*
 * Script 2009, Jeremy Peterson, http://4volt.com/blog/
 * SoftwareSerial.h 2009 Todd E. Kurt, 	http://todbot.com
 * 				       	http://todbot.com/arduino/sketches/SoftwareServoKnob/SoftwareServo.cpp
 *				       	http://todbot.com/arduino/sketches/SoftwareServoKnob/SoftwareServo.h
 *					http://www.arduino.cc/playground/ComponentLib/Servo 					
 */

#include "SoftwareServo.h"

SoftwareServo testServo;

int servoMaxPulse;

void setup()
{
  testServo.attach(2); // your servo pin here
  Serial.begin(9600);
          
  Serial.println("Starting test");
}

void loop()
{
  servoMaxPulse = 2400;
  
  testServo.setMaximumPulse(servoMaxPulse);
  delay(5000);
        
  Serial.println("Rotate Test 90, 2 sec, a correctly adjusted servo should be stationary");
  spinServo(2000, 90);
  
  Serial.println("Rotate Test 0, 2 sec");
  spinServo(2000, 0);

  Serial.println("Rotate Test 180, 2 sec");  
  spinServo(2000, 180);

  int range = 10;

  for(range = 10; range < 5000; range += 50){
    Serial.print("Max pulse test: ");  
    Serial.print(range);  

    int thisMaxPulse = servoMaxPulse;

    for(thisMaxPulse = (servoMaxPulse - range); thisMaxPulse < (servoMaxPulse + range); thisMaxPulse += 1){
      Serial.println(thisMaxPulse); 
      testServo.setMaximumPulse(thisMaxPulse);
      spinServo(250, 90);
    }
    
  }


  
}

void spinServo(int ForMills, int Rotation) {
  int i = 0;
  
  for(i = 0; i < ForMills; i += 1){
    testServo.write(Rotation);
    SoftwareServo::refresh();
    delay(1);
  }
}