//-----------------------------------------------------------------
// File:		PBot_RateServo_Test_VC6.osc		
//
// Project:		Sample program to use PBot_RateServo_1.0_VC6.osc oUserClass;
//
// Designer:	Phil Malone,  www.philbot.com
// Details:		http://www.philbot.com/projects/RateServo
//
// Details:		Rate controled servo for smooth control or actuator
//				.... or so I hoped
//
// Comments:	oServoX object isn't capable of small movement intervals
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00    8/22/2005	First release for VC6
//
//-----------------------------------------------------------------

//-----------------------------------------------------------------
// System Constants
//-----------------------------------------------------------------
Const	IO_SERVO_UP		= 9  ;		// Servo used to move arm Up/Down
Const	IO_SERVO_FWD	= 10 ;		// Servo used to move Fwd/Back

//-----------------------------------------------------------------
// System Objects: Total Object Memory Used: 60 Bytes
//-----------------------------------------------------------------
// Define two rate servos
oUserClass	Servo_Up	= New oUserClass("PBot_RateServo_1.0_VC6.osc");
oUserClass	Servo_Fwd	= New oUserClass("PBot_RateServo_1.0_VC6.osc");


//-----------------------------------------------------------------
//	Setup two servos and move them in coordinated motion
//-----------------------------------------------------------------
Void Main(Void)
{
	Servo_Up.SetupServo(IO_SERVO_UP, 0);	// Define IO and position of up servo
	Servo_Fwd.SetupServo(IO_SERVO_FWD, 0);	// Define IO and position of fwd servo

	// Repeat a coordnated motion
	While( cvTrue)
	{
		// Move arm up and forward quickly
		//  Up 45 degrees at 15 degrees per second
		//	Fwd 90 degrees at 30 degrees per second
		Servo_Up.MoveTo(32, 6);		// (64 * 90 / 45) , (90 / 15)
		Servo_Fwd.MoveTo(64, 3);	// (64 * 90 / 90) , (90 / 30)

		// Wait for motion to complete
		While(Servo_Up.mMoving.Operate && Servo_Fwd.mMoving.Operate);		

		// Move arm down and back slowly
		//  Down 45 degrees at 5 degrees per second
		//	Back 90 degrees at 10 degrees per second
		Servo_Up.MoveTo(-32, 18);	// (64 * 90 / -45) , (90 / 5)
		Servo_Fwd.MoveTo(-64, 9);	// (64 * 90 / -90) , (90 / 10)

		// Wait for motion to complete
		While(Servo_Up.mMoving.Operate && Servo_Fwd.mMoving.Operate);		
	}
}