//-----------------------------------------------------------------
// Program:		PBot_BOB-F_VC6.osc
// Function:	Lesson F. Closed Loop Wheel Position Virtual Circuit
// Designer:	Phil Malone,  phil@philbot.com  www.philbot.com 
// Details:		http://www.philbot.com  
//
// Device:		Bot On a Board (BOB) V1.1  
// Compiler:	OOPic 6.x  <<<  Will not compile with 5.0 compiler
// Motor Drive:	SN75410 With Tamiya Gearbox
// Quad encoder:OPB610  
//
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00    8/17/2005	Original
//-----------------------------------------------------------------

//-------------------------------------------------------
// BOB I/O Port definitions
//-------------------------------------------------------
Const	IO_LINE_LEFT			= 3;	// Left Fairchild Analog Reflective sensor
Const	IO_LINE_MID				= 2;	// Middle Fairchild Analog Reflective sensor
Const	IO_LINE_RIGHT			= 1;	// Right Fairchild Analog Reflective sensor

Const	IO_PROXIMITY_LEFT		= 8;	// Left Sharp Digital Prox sensor
Const	IO_PROXIMITY_RIGHT		= 9;	// Right Sharp Digital Prox sensor
Const	IO_PROXIMITY_REAR		= 10;	// Rear Sharp Digital Prox sensor

Const	IO_LED1					= 12;	// General Purpose LED 1
Const	IO_LED2					= 13;	// General Purpose LED 2
Const	IO_LED3					= 14;	// General Purpose LED 3
Const	IO_LED4					= 15;	// General Purpose LED 4
Const	IO_LED_GROUP			= 1;	// LED I/O Group 1    (Lines 8-15)
Const	IO_LED_NIBBLE			= 1;	// LED I/O High Nible (Lines 12-15)

Const	IO_MOTOR_LEFT_FWD		= 24;	// Left Motor H-Bridge High
Const	IO_MOTOR_LEFT_REV		= 25;	// Left Motor H-Bridge Low
Const	IO_MOTOR_LEFT_PWM		= 18;	// Left Motor PWM control

Const	IO_MOTOR_RIGHT_FWD		= 26;	// Right Motor H-Bridge High	
Const	IO_MOTOR_RIGHT_REV		= 27;	// Right Motor H-Bridge Low	
Const	IO_MOTOR_RIGHT_PWM		= 17;	// Right Motor PWM control

Const	IO_ENCODER_LEFT_FRONT	= 31;	// Left A I/R optical interruptor
Const	IO_ENCODER_LEFT_BACK	= 30;	// Left B I/R optical interruptor
Const	IO_ENCODER_RIGHT_FRONT	= 29;	// Right A I/R optical interruptor
Const	IO_ENCODER_RIGHT_BACK	= 28;	// Right B I/R optical interruptor

//-----------------------------------------------------------------
// Constants, used for default values
//-----------------------------------------------------------------
Const	CLOCK_PRESCALE		=   5;	  	// Divide Clock by 6
Const	FIVE_SECONDS		=  50;	  	// 50 tenths
Const	SLOW_PWM			=   2;	  	// Maximum PWM divide for 312KHz
Const	PWM_PERIOD			= 100;		// Maximum PWM +/- range
Const	OPPONENT			=  cvLow; 	// Eye Sees Opponent	
Const	CLEAR				=  cvHigh;	// Eye Does Not see Obstacle	

Const	LEFT_WHEEL			=  1;		// Used when passing motion setpoints
Const	RIGHT_WHEEL			=  2;
Const	BOTH_WHEELS			=  3;

Const	NEUTRAL				=  0;		// Used to set wheel direction
Const	FORWARD				=  1;
Const	REVERSE				=  2;

Const	SPIN_90				=  9;		// Determined by experimentation
Const	INCHES_10			= 50;		// Determined by experimentation

//-----------------------------------------------------------------
// User Objects.  Use these to access BOB hardware.
//-----------------------------------------------------------------
oCountDownO ActionTimer		= New oCountDownO; 	// User Timer	

// Wheel sensor and driver Objects
oQencode 	LeftClicks   	= New oQencode;		// Value for Left Encoder
oQencode 	RightClicks   	= New oQencode;		// Value for Right Encoder

// LED Object (accessed as individual bits)
oDIO1 		LeftRunning	   	= New oDIO1; 		// Driver for LED1
oDIO1 		RightRunning   	= New oDIO1; 		// Driver for LED4

// Wheel motor driver Objects
oDCMotor2 	LeftMotor   	= New oDCMotor2; 	// Driver for the Left Wheel
oDCMotor2 	RightMotor   	= New oDCMotor2; 	// Driver for the Right Wheel

//-----------------------------------------------------------------
// Virtual Circuit objects
//-----------------------------------------------------------------
oCompare0	LeftMatch		= New oCompare0;	// Used to detect end-of-motion
oWire		LeftConnect		= New oWire;		// Used to trigger Stop Event	
oEvent		LeftAutoStop	= New oEvent;		// Motion Stop Object

oCompare0	RightMatch		= New oCompare0;	// Used to detect end-of-motion
oWire		RightConnect	= New oWire;		// Used to trigger Stop Event	
oEvent		RightAutoStop	= New oEvent;		// Motion Stop Object

//-----------------------------------------------------------------
//	BOB Main Program
//-----------------------------------------------------------------
Void Main(Void)
{
	// Always set up the required IO and VC's first.
	SetupIO();
	SetupVC();
	
	// Wait for 5 second "Start Up" to elapse
	While(ActionTimer.NonZero);

	// Drive in a square
	While (cvTrue)
	{
		// Turn 90 degrees clockwise
		SetWheelPosition(LEFT_WHEEL,  FORWARD,  SPIN_90,  50);
		SetWheelPosition(RIGHT_WHEEL, REVERSE,  SPIN_90,  50);
		While (WheelsMoving()) ;
		WaitTenths(5);
		
		// Drive forward 10 Inches
		SetWheelPosition(BOTH_WHEELS, FORWARD, INCHES_10,  50);
		While (WheelsMoving()) ;
		WaitTenths(5);
	}
}

//#################################################################
// These are the standard functions that All lessons have
//#################################################################

//-----------------------------------------------------------------
//	SetupIO()
//	Configure all the IO Objects here
//-----------------------------------------------------------------
Void	SetupIO( Void )
{
	// --------------------------------
   	// setup the ActionTimer Timer for 1/10th Second ticks 
	// Preload counter with 5 second delay
	// --------------------------------
	ActionTimer.PreScale	= CLOCK_PRESCALE;
	ActionTimer.ClockIn.Link(ooPIC.Hz60);
	ActionTimer				= FIVE_SECONDS;
	ActionTimer.Operate		= cvTrue;

	// --------------------------------
   	// setup the Wheel Encoders
	// --------------------------------
	LeftClicks.Value	= 0;
	LeftClicks.IOLine1	= IO_ENCODER_LEFT_FRONT ;
	LeftClicks.IOLine2	= IO_ENCODER_LEFT_BACK ;
	LeftClicks.Operate	= cvTrue;

	RightClicks.Value	= 0;
	RightClicks.IOLine1	= IO_ENCODER_RIGHT_FRONT ;
	RightClicks.IOLine2	= IO_ENCODER_RIGHT_BACK ;
	RightClicks.Operate	= cvTrue;
	
	// --------------------------------
   	// setup the LED Drivers
	// --------------------------------
	LeftRunning.IOLine		= IO_LED1;
	LeftRunning.Direction	= cvOutput ;
	RightRunning.IOLine		= IO_LED4;
	RightRunning.Direction	= cvOutput ;

	// --------------------------------
   	// setup the Wheel Drivers
	// --------------------------------
	LeftMotor.Value		= 0;
	LeftMotor.PreScale	= SLOW_PWM;		
	LeftMotor.Mode		= cvOn;		// Active Braking
	LeftMotor.Brake		= cvOff;
	LeftMotor.IOLine1	= IO_MOTOR_LEFT_FWD;
	LeftMotor.IOLine2	= IO_MOTOR_LEFT_REV;
	LeftMotor.IOLineP	= IO_MOTOR_LEFT_PWM;
	LeftMotor.Unsigned	= cvFalse;
	LeftMotor.Period	= PWM_PERIOD;
	LeftMotor.Operate	= cvTrue;
	
	RightMotor.Value	= 0;
	RightMotor.PreScale	= SLOW_PWM;
	RightMotor.Mode		= cvOn;		// Active Braking
	RightMotor.Brake	= cvOff;
	RightMotor.IOLine1	= IO_MOTOR_RIGHT_FWD;
	RightMotor.IOLine2	= IO_MOTOR_RIGHT_REV;
	RightMotor.IOLineP	= IO_MOTOR_RIGHT_PWM;
	RightMotor.Unsigned	= cvFalse;
	RightMotor.Period	= PWM_PERIOD;
	RightMotor.Operate	= cvTrue;
}


//-----------------------------------------------------------------
//	SetupVC()
//	Configure all the Virtual Circuit Objects here
//-----------------------------------------------------------------
Void	SetupVC(Void)
{
	// --------------------------------
   	// Setup the Closed Loop VC's
	// --------------------------------
	LeftMatch.Input.Link(LeftClicks);				// Setpoint comparison
	LeftMatch.Fuzziness 	= 0;					// Tolerated error
	LeftMatch.Operate		= cvTrue;				// Run
	LeftConnect.Input.Link(LeftMatch.Between);		// Connect Match to Stop Event
	LeftConnect.Output.Link(LeftAutoStop.Operate);

	RightMatch.Input.Link(RightClicks);				// Setpoint comparison
	RightMatch.Fuzziness 	= 0;					// Tolerated error
	RightMatch.Operate		= cvTrue;				// Run// Tollerated error
	RightConnect.Input.Link(RightMatch.Between);	// Connect Match to Stop Event
	RightConnect.Output.Link(RightAutoStop.Operate);
}

//-----------------------------------------------------------------
//	EVENT SETUP SUBROUTINES
//-----------------------------------------------------------------
Sub Void SetWheelPosition(Byte Wheel, Byte Direction, Word Clicks, Byte Speed)
{
	If (Direction == FORWARD)
		Clicks = 0 - Clicks;
	Else
		Speed = 0 - Speed ;		
			
	// Which Wheel ?
	If ((Wheel & LEFT_WHEEL) == LEFT_WHEEL)
	{
		LeftClicks = Clicks;			// Set the click countdown
		LeftConnect.Operate = cvTrue ;	// Turn On event trigger
		LeftMotor.Brake = cvOff  ;  	// Release the Brake
		LeftMotor = Speed ;				// Start the rotation
		LeftRunning.Set ;				// Signal Running
	}
		
	If ((Wheel & RIGHT_WHEEL) == RIGHT_WHEEL)
	{
		RightClicks = Clicks;			// Set the click countdown
		RightConnect.Operate = cvTrue ;	// Turn On event trigger
		RightMotor.Brake = cvOff  ;  	// Release the Brake
		RightMotor = Speed ;			// Start the rotation
		RightRunning.Set;				// Signal Running
	}
}

//-----------------------------------------------------------------
//	WheelsMoving()
//	This function returns true if any wheels are still turning
//	
//-----------------------------------------------------------------
Function Bit	WheelsMoving(Void)
{
	WheelsMoving = (LeftRunning | RightRunning);	
}
	
//-----------------------------------------------------------------
//	EVENT SUBROUTINES
//  These are called by the two oEvent objects when 
//	their Operate inputs are set.
//-----------------------------------------------------------------

Sub Void LeftAutoStop_Code(Void)
{
	LeftMotor = - LeftMotor  ;			// Reverse the motor drive
	LeftMotor = 0  ;					// Stop the motor drive
	LeftMotor.Brake = cvOn  ;			// Brake the motor drive
	LeftConnect.Operate = cvFalse ;		// Disable the event trigger
	LeftRunning.Clear ;					// Signal NotRunning
}

Sub Void RightAutoStop_Code(Void)
{
	RightMotor = - RightMotor  ;		// Reverse the motor drive
	RightMotor = 0  ;					// Stop the motor drive
	RightMotor.Brake = cvOn  ;			// Brake the motor drive
	RightConnect.Operate = cvFalse ;	// Disable the event trigger
	RightRunning.Clear ;				// Signal NotRunning
}

//-----------------------------------------------------------------
//	WaitTenths(Byte Tenths)
//	This function uses the Action timer to wait for a number
//	of "tenths" of a second.  
//	This method is more accurate than the OOPic.Delay function
//-----------------------------------------------------------------
Void WaitTenths(Byte Tenths)
{
	// Set timer
	ActionTimer = Tenths;
	While(ActionTimer.NonZero);
}