//-----------------------------------------------------------------
// File:	PBot_RateServo_1.0_VC6.osc		
//
// Project:	Sample oUserClass for servo rate control
//
// Designer:	Phil Malone,  www.philbot.com
// Details:		http://www.philbot.com/projects/RateServo
//
// Details:		Rate controled servo for smooth control or actuator
//				See Bot_RateServo_Test_VC6.osc for sample two-servo system
//
// Comments:
//
//	This provide a two parameter servo controller.
//	The user controls the servo action by setting the Position and Rate
//	Position is a URCP value with a range of +/- 90Deg.
//	Rate is a divider function:  It sets how many seconds it takes to move 90 Deg.
//
//	Rate = 1 means 90 Degree motion in 1 Second  (90 dps)
//	Rate = 2 means 90 Degree motion in 2 Seconds (45 dps)  	
//	Rate = 3 means 90 Degree motion in 3 Seconds (30 dps)
//	Rate = 4 means 90 Degree motion in 4 Seconds (22.5 dps)
//	etc.
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00    8/22/2005	First release for VC6
//
//-----------------------------------------------------------------

//-----------------------------------------------------------------
// System Constants
//-----------------------------------------------------------------
Const	SERVO_MIDPOINT	= 64 ;

//-----------------------------------------------------------------
// System Objects: Total Object Memory Used: 30 Bytes
//-----------------------------------------------------------------
// Device & Virtual Circuit Objects
oDivider	mRate			= New oDivider;		// Divides down 60hZ clock
oByte		mPosition  		= New oByte;		// Target Position for Servo

oBit		mClock		 	= New oBit;			// Holds Counter Clock State
oServoX		mServo			= New oServoX;		// Servo Object
oCompare	mComparison	 	= New oCompare;		// Decides which way to move the Servo
oWire		mEnable			= New oWire;		// Used to enable/disable counter
oWire		mDirection		= New oWire;		// Used to transfer direction control
oCounter	mMoving	 	= New oCounter;		// Updates Servo Value


//-----------------------------------------------------------------
//	RateServo User Defined Object Constructor
//-----------------------------------------------------------------
Void Main(Void)
{
	SetupVC();							// Setup Virtual Circuits
	//	ClassTest();					// Test Code:	
}

//-----------------------------------------------------------------
//	ClassTest()
//	Verify that one instance of this Object works
//-----------------------------------------------------------------
Void	ClassTest(Void)
{
	// Test code
	SetupServo(9,0);					// Connect Servo to IO Line 9
	
	MoveTo( 64,3);						// Goto +90 Deg @ 30dps
	While (mMoving.Operate);			// Wait for motion to end

	MoveTo(  0,3);						// Return to 0 Deg @ 30dps
	While (mMoving.Operate);			// Wait for motion to end
	
	MoveTo(-32,6);						// Go to -45 Deg @ 15 dps
	While (mMoving.Operate);			// Wait for motion to end

	MoveTo(  0,9);						// Return to 0 Deg @ 10 dps
	While (mMoving.Operate);			// Wait for motion to end

	MoveTo( 10,15);						// Go to 9 Deg @ 6 dps
	While (mMoving.Operate);			// Wait for motion to end

	MoveTo( 0,1);						// Return to 0 deg @ 90 dps
}

//-----------------------------------------------------------------
//	SetupServo(IOLine, Position)
//	Connect this Object to a servo, and move to the desired position
//-----------------------------------------------------------------
Void	SetupServo(Byte IOLine, Byte Position)
{
	mServo.IOLine	=	IOLine;			// Attach to the correct IO Line
	mServo.Center	=	0;				// Null out center
	mServo.OffSet	=	SERVO_MIDPOINT;	// MOVE SERVO INTO 0-127 RANGE
	mServo.Operate	= 	cvTrue;			// Enable Servo
}

//-----------------------------------------------------------------
//	MoveTo(Position, Rate)
//	Move this object's servo to the desired position, at the desired rate
//  Position is +/- 64
//-----------------------------------------------------------------
Void	MoveTo(Byte Position, Byte Rate)
{
	mRate.Rate	=	Rate;
	mPosition	=	Position + SERVO_MIDPOINT;
}


//-----------------------------------------------------------------
//	SetupVC()
//	Configure all the Virtual Circuits
//-----------------------------------------------------------------
Void	SetupVC(Void)
{
	mPosition 		= SERVO_MIDPOINT;		// Set Desired position to zero
	mServo			= SERVO_MIDPOINT;		// Set Servo to Null

	// --------------------------------
   	// Setup Clock Divider
	// --------------------------------
	mRate.ClockIn.Link(ooPIC.Hz60);			// Divide down the 60Hz clock
	mRate.Output.Link(mClock);				// Output to storage Bit
	mRate.Rate 			= 1;				// 90 Degrees per second
	mRate.Operate		= cvTrue;			// Run		
	
	// --------------------------------
   	// Setup compare for deciding direction
	// --------------------------------
	mComparison.Input.Link(mServo);			// Compare the current value of the servo
	mComparison.ReferenceIn.Link(mPosition);// With the setpoint position
	mComparison.Fuzziness	= 0 ;			// Look for exact match
	mComparison.Operate 	= cvTrue;		// Run		
	
	// --------------------------------
   	// Setup wire to transfer counter enable
	// --------------------------------
	mEnable.Input.Link(mComparison.Between);	// Take the position Match state
	mEnable.InvertIn		= cvTrue;			// Invert it	
	mEnable.Output.Link(mMoving.Operate);		// and feed it to the counter enable
	mEnable.Operate		= cvTrue;				// Run	
	
	// --------------------------------
   	// Setup wire to transfer Counter Direction
	// --------------------------------
	mDirection.Input.Link(mComparison.Above);	// Take the position comparison
	mDirection.Output.Link(mMoving.Direction);	// and feed it to the counter Direction
	mDirection.Operate		= cvTrue;			// Run	
	
	// --------------------------------
   	// Setup Counter
	// --------------------------------
	mMoving.Mode		= cvCount;				// Set direction based on "Direction"
	mMoving.Tick		= 0;					// Change count by 1.
	mMoving.ClockIn1.Link(mClock);				// Count based on divided clock bit	
	mMoving.Output.Link(mServo);				// Update the servo value	
}