// Manual servo control - PWM Sweeping
// Copyright 2012 by Zipwhip. 
// Modified July 2013 by LinuxCircle.com team
// You are free to use and modify this code in your own software.
// Please give us credit to by mentioning LinuxCircle.com 
// in the header of your published code.

#define SERVO_PIN         9  // Any pin on the Arduino or Gertboard will work.

void setup()
{
  pinMode(SERVO_PIN, OUTPUT);

}
int degree1=70;
int degree2=80;
float function1=(degree1*1.5)/90;
float function2=(degree2*1.5)/90;
int lenMicroSecondsOfPeriod = 80 * 1000; // 20 milliseconds (ms)
int lenMicroSecondsOfPulse = 1 * 1000; // 1 ms is 0 degrees
int first = function1 * 1000; //0.5ms is 0 degrees in HS-422 servo
int end = function2 * 1000;
int increment = 0.01 * 1000;

void loop()
{
    int current = 0;
    for(current = first; current <end; current+=increment){
         // Servos work by sending a 25 ms pulse.  
         // 0.7 ms at the start of the pulse will turn the servo to the 0 degree position
         // 2.2 ms at the start of the pulse will turn the servo to the 90 degree position 
         // 3.7 ms at the start of the pulse will turn the servo to the 180 degree position 
         // Turn voltage high to start the period and pulse
         digitalWrite(SERVO_PIN, HIGH);

         // Delay for the length of the pulse
         delayMicroseconds(current);

         // Turn the voltage low for the remainder of the pulse
         digitalWrite(SERVO_PIN, LOW);

         // Delay this loop for the remainder of the period so we don't
         // send the next signal too soon or too late
         delayMicroseconds(lenMicroSecondsOfPeriod - current); 
    }
    for(current = end; current >first; current-=increment){
         // Servos work by sending a 20 ms pulse.
         // 0.7 ms at the start of the pulse will turn the servo to the 0 degree position
         // 2.2 ms at the start of the pulse will turn the servo to the 90 degree position
         // 3.7 ms at the start of the pulse will turn the servo to the 180 degree position
         // Turn voltage high to start the period and pulse
         digitalWrite(SERVO_PIN, HIGH);

         // Delay for the length of the pulse
         delayMicroseconds(current);

         // Turn the voltage low for the remainder of the pulse
         digitalWrite(SERVO_PIN, LOW);

         // Delay this loop for the remainder of the period so we don't
         // send the next signal too soon or too late
         delayMicroseconds(lenMicroSecondsOfPeriod - current);
    }

}