/*********************
*Calibrate Servo Program
2005-01-08
Assuming that Servos are plugged into Servo Ports 0 (Left Servo)
and Servo Port 3 (Right Servo)

*********************/

/* Approx value for stopped Servo
It will remain stopped over a range of values
To conserve power try and get this accurate */

#define STOPPED 2480 

/* UPPER and LOWER set the limit values for the test */

#define UPPER 2650
#define LOWER 2350

void main()
{
    printf("\n");
    printf("Note Value when Wheels stop \n");
    beep();
    sleep(2.0);
    motors();
    printf("Program End");
    sleep(10.0);
}

void motors(void)
{
    int i;
    int j;
    
    
        init_servos(1);
        for (i=LOWER;(!stop_button() && (i <UPPER)); i=i+5)
           {
           //stop servo 3 
           servo3=STOPPED;
           printf("Left Wheel      Servo0=%d\n",i);
           servo0=i;
           sleep(0.25);

            }
        
        init_servos(0);
        beep();
    
        sleep(5.0);
        
        init_servos(1);
        for (j=UPPER;(!stop_button() && (j >LOWER)); j=j-5)
            {
             //stop servo 0 
            servo0=STOPPED;
            printf("Right Wheel     Servo3=%d\n",j);
            servo3=j;
            sleep(0.25);

             }
        init_servos(0);
        beep();
        sleep(5.0);
             
      
    }