//-----------------------------------------------------------------
// PBot Sumo Program using PBotMkIII_VC User Class
//
// Designer:	Phil Malone,  www.philbot.com
// Details:	http://www.philbot.com/software_OOPic_Sumo_VC.htm
//
// Details:	Mode 1: Defensive Unit. (Line Avoid Priority)
//
//	Mode 1 is a "Safe" 3 priority Sumo Stance. 	
//
//	Priority 1: Avoid the edge of the Ring, using backup and spin.
//	Priority 2: Turn towards, and follow the opponent.
//	Priority 3: Move forward. 
//
//      Basic program flow is:
//	
//	1) Wait 4 seconds
//	2) Calibrate the line sensors 
//	3) Wait for remainder of 5 second pause
//	4) Activate the Servos
//	5) Run the main action-priority loop
//
// Comments:
//
//	This program is fooled by an opponent that has a white line 
//	on it's pusher bar.  However it won't be fooled by an object
//	or person outside the ring.
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00     	5/12/2005	First release
//
//-----------------------------------------------------------------

// Create motion constants
Const	FAST_FORWARD	=  10;
Const	SLOW_FORWARD	=   1;
Const	FAST_REVERSE	= -10;
Const	FAST_RIGHT	=  10;
Const	FAST_LEFT	= -10;

// Create instance of MKIII Object.  
// Note: Make sure the path matches your file locations.
oUserClass 	MkIII		= New oUserClass ("C:\OOPIC\SUMO\PBot_MkIII_VC.osc");

oCountDownO	ActionTimer	= New oCountDownO;

// Main program
Sub Void	Main(Void) 
{ 
	// --------------------------------
   	// setup the Action Timer for 1/10th Second ticks 
	// Preload counter with 5 second delay
	// --------------------------------
	ActionTimer.PreScale	= 5;
	ActionTimer.ClockIn.Link(ooPIC.Hz60);
	ActionTimer		= 50;
	ActionTimer.Operate	= cvTrue;

	// Count down to 1 second then Calibrate line sensors  
	While(ActionTimer > 10) ;
	MkIII.CalibrateLine;

	// Wait for last second to expire
	While(ActionTimer.NonZero) ;

	// Enable Servo drive
	MkIII.StartServos; 

	// Repeat main program loop forever.
	While (cvTrue) 
	{
		// Run down the SUMO priority list

		// Priority 1: Avoiding the border
		If (MkIII.LeftLineOn | MkIII.CenterLineOn)
		{
			// If we see the left line, backup & Spin right 180
			MkIII.Forward(FAST_REVERSE); 
			ooPIC.Delay = 25;
			MkIII.Spin(FAST_RIGHT); 
			ooPIC.Delay = 45;
		}
		Else If (MkIII.RightLineOn)
		{
			// If we see the right line, backup and spin left 180
			MkIII.Forward(FAST_REVERSE); 
			ooPIC.Delay = 25;
			MkIII.Spin(FAST_LEFT); 
			ooPIC.Delay = 45;  
		}
		
		// Priority 2: Follow the opponent
		Else If (MkIII.BothEyesOn)
		{
			// Go forward
			MkIII.Forward(FAST_FORWARD); 
		}
		Else If (MkIII.RightEyeOn)
		{
			// Turn right towards the target
			MkIII.Drive(SLOW_FORWARD,FAST_RIGHT); 
		}
		Else If (MkIII.LeftEyeOn)
		{
			// Turn left towards the target
			MkIII.Drive(SLOW_FORWARD,FAST_LEFT); 
		}

		// Priority 3:  Go Forward
		Else
		{
			// Go forward
			MkIII.Forward(FAST_FORWARD); 
		}
	}
}