/**************************************************************/
/*                                                            */
/* servoExample.c - an example of how to use the              */
/*     dual servo routines of interactive c                   */
/*                                                            */
/* Author:                                                    */
/*     Roy Merrell                                            */
/*     ME 3200/3210                                           */
/*     12 November 2002                                       */
/*                                                            */
/* info about Handy Board servo subroutines                   */
/* http://handyboard.com/software/servo.html                  */
/*                                                            */
/* info about servos in general                               */
/*http://documents.epanorama.net/documents/motor/rcservos.html*/
/*http://www.geocities.com/BourbonStreet/3220/servobasics.html*/
/*                                                            */
/**************************************************************/

void main(void) {

	/* variable declarations */
		
	/* min = minimum servo control value */
	/* max = maximum servo control value */
	/* you will need to experiment to determine */
	/* the min and max values of your servo */
	/* mine were min=650 max=4700 */
	/* if you command the servo past its limits */
	/* the motor will stall on its stops and might fail */
	float min=650.,max=4700.;
	
	/* x = knob value */
	float x;
	
	/* m = calibration coefficient from knob to servo control */
	float m = (max-min)/255.;
	
	/* b1 = offset value 1 */
	float b1 = min;
	
	/* b2 = offset value 2 */
	float b2 = max;
	
	/* variable to hold center position of servo */
	int center=(int)((min+max)/2.);
	
	/* initialize the servo subroutines */
	/* these subroutines are in servo_a5.icb */
	/* and servo_a7.icb */
	/* the initialization need only be called once */
	/* 1 turns the servo on 0 turns it off */
	servo_a5_init(1);
	servo_a7_init(1);
	
	/* the initialization subroutines set the servo to */
	/* position 2570, if a different initial position */
	/* is desired the global position variable must be changed */
	/* the following lines set the servos to the center of */
	/* of their range */
	servo_a5_pulse = center;
	servo_a7_pulse = center;
	
	while(1) {
		
		/* hold until start is pressed */
		printf("start to controlservo with knob\n");
		start_press();
		
		/* loop continously */
		while(1) {
			
			/* servo_aX_pulse is a global variable */
			/* defined in servo_aX.icb */
			/* you will need to experiment to determine */
			/* the min and max values of your servo */
			/* mine were min=650 max=4700 */
			/* if you command the servo past its limits */
			/* the motor will stall on its stops and might fail */
			x = (float)knob();
			servo_a5_pulse = (int)(m*x+b1);
			servo_a7_pulse = (int)(-m*x+b2);
			
			/* print the command values */
			if(servo_a5_pulse>1000) {
				printf("a5 = %d       a7 = %d\n",servo_a5_pulse,servo_a7_pulse);
			}
			/* when servo_a5_pulse only has 3 digits */
			/* add a space to make it print nicer */
			else printf("a5 = %d        a7 = %d\n",servo_a5_pulse,servo_a7_pulse);
			
			/* hold for .1 seconds so screen doesn't flash */
			sleep(.1);
			
			/* exit if the stop button is pushed */
			if (stop_button()) break;
		}
		/* turn the servos off by calling servo_aX_init(0); */
		servo_a5_init(0);
		servo_a7_init(0);
	}
}