//-----------------------------------------------------------------
// oUserClass: PBot_MkIII_1.0_VC6.osc (V6.0 Virtual Circuit)
//
// Designer:	Phil Malone,  phil@philbot.com  www.philbot.com
// Details:		http://www.philbot.com/projects/markIII_sumo
//
// Device:	MARKIII MINI-SUMO ROBOT 
// Servos:	GWS S03N
// Eyes:	Sharp GP2D12 IR Distance Measuring Sensor
// Lines:	Fairchild QRB1134 IR Photoreflector
//
// PBotMkIII_VC is an OOPIC oUserClass object to encapsulate the 
// functions of a MarkIII Mini-Sumo Robot.  
// Diagnostics may be added by wiring LED's to I/O lines 24-28
//  - Use a series 220 Ohm resistor to ground on each LED.
//
// The Class sets up all the I/O channels and Virtual Circuits.
//
// Boolean flags indicate when each sensor is detecting its target.
//
//	LeftEyeOn				True if Left eye sees opponent
//	RightEyeOn				True if Right eye sees opponent
//	LeftLineWhite			True if Left line sensor sees white
//	CenterLineWhite  		True if Center line sensor sees white
//	RightLineWhite  		True if Right line sensor sees white
//
// Subroutones are provided to calibrate the inputs and set the 
// MarkIII in motion.
//
//	EnableDiagnostics()		Call to power up LEDs
//	FollowerCalibrate()		Calibrate when centered over a line to be followed
//	SumoCalibrate()			Calibrate when on black Sumo Ring
//	Drive(Left Speed, Right Speed, Duration)	Turn wheels as indicated
//	WaitTenths(Tenths)		Pause for a number of "tenths of seconds"
//	ObjectTest()			Call this to test the drive and display Status LEDS
//
// This oUserClass does NOT decide what actions the Bot should take 
// in response to changing sensor inputs, this must be done by the 
// calling program.  This way, many different fighting approaches
// may be tried.
//
//	Include this line to Create instance of MkIII Object.  
//	oUserClass BOT = New oUserClass("PBot_MkIII_1.0_VC6.osc");
//
//  This oUserClass is written for OOPIC Compiler V6.0	
//	It will NOT compule on the earlier 5.0 compiler
//	See www.PhilBot.com/downloads.htm for a 5.0 version.
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00   	8/22/2005	First release
//
//-----------------------------------------------------------------

//-----------------------------------------------------------------
// System Constants
//-----------------------------------------------------------------
// I/O Definitions
Const		IO_EYE_LEFT		= 4;
Const		IO_EYE_RIGHT	= 3;
Const		IO_LINE_LEFT	= 7;
Const		IO_LINE_CENTER	= 6;
Const		IO_LINE_RIGHT	= 5;

Const		IO_WHEEL_LEFT	= 10;
Const		IO_WHEEL_RIGHT	= 9;

Const		IO_DEBUG_LED_1	= 24;
Const		IO_DEBUG_LED_2	= 25;
Const		IO_DEBUG_LED_3	= 26;
Const		IO_DEBUG_LED_4	= 27;
Const		IO_DEBUG_LED_5	= 28;

// Usefull values
Const		CLOCK_PRESCALE	=   5;	// Divide Clock by 6
Const		FIVE_SECONDS	=  50;	// 50 tenths
Const		ONE_SECOND		=  10;	// 10 tenths

Const   	WHEEL_OFFSET	= 18;	// Count for Full Reverse
Const	   	WHEEL_STOP		= 36;	// Null position for servo

Const	   	ZERO_OFFSET		= 127;	// A2DX Offset for all positive values
Const		EYE_OFFSET		= 25;	// Corrosponds to 70cm range (0.5V)

//-----------------------------------------------------------------
// System Objects: Total Object Memory Used:  55 Bytes
//-----------------------------------------------------------------
// Device Objects
oCountDownO ActionTimer		= New oCountDownO;	// User Timer	

oDIO1		LeftEyeOn	 	= New oDIO1;		// Status Bits/LEDs
oDIO1		LeftLineWhite 	= New oDIO1;
oDIO1		CenterLineWhite	= New oDIO1;
oDIO1		RightLineWhite 	= New oDIO1;
oDIO1		RightEyeOn	 	= New oDIO1;

oA2DX		LeftEye   		= New oA2DX;		// Sharp I/R Range
oA2DX		RightEye   		= New oA2DX;
oA2D		LeftLine  		= New oA2DX;		// Fairchild I/R Reflective
oA2D		CenterLine  	= New oA2DX;
oA2D		RightLine  		= New oA2DX;

oServo		LeftWheel 		= New oServo;		// Cont. Rot. Servos
oServo		RightWheel		= New oServo;

// Virtual Circuit objects
oWire	  	LeftEyeWire		= New oWire;		// VC Connectors to Drive LEDs
oWire	  	LeftLineWire	= New oWire;
oWire	  	CenterLineWire	= New oWire;
oWire	  	RightLineWire	= New oWire;
oWire	  	RightEyeWire	= New oWire;

//-----------------------------------------------------------------
// Public Variables
//-----------------------------------------------------------------
Word		TempWord;

//-----------------------------------------------------------------
//	MarkIII User Defined Object Constructor
//-----------------------------------------------------------------
Void Main(Void)
{
	SetupIO();
	SetupVC();
	
	// Run a test of this Object
	// ObjectTest();
}

//-----------------------------------------------------------------
//	UserClassTest()
//	Run a basic test then stop.
//-----------------------------------------------------------------
Void	ObjectTest()
{
	// Sample Test Program
	While(ActionTimer.NonZero);		// Wait for 5 seconds to expire
	SumoCalibrate();				// Calibrate for Sumo
	EnableDiagnostics();			// Turn On Status LEDs
	Drive(10, 10, 10);				// Drive forward for 1 second
	Drive(0, 0, 0);					// Stop and now show STATUS LEDs
}

//-----------------------------------------------------------------
//	SetupIO()
//	Configure all the IO devices
//-----------------------------------------------------------------
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 Eye sensors
	// We use oA2DX so we can bias the values up to permit single bit checking
	// --------------------------------
   	LeftEye.IOLine 			= IO_EYE_LEFT;
   	LeftEye.Center			= ZERO_OFFSET - EYE_OFFSET;	// Bias the level up
   	LeftEye.Operate 		= cvTrue;

   	RightEye.IOLine 		= IO_EYE_RIGHT;
   	RightEye.Center			= ZERO_OFFSET - EYE_OFFSET;	// Bias the level up
   	RightEye.Operate 		= cvTrue;

	// --------------------------------
   	// setup the line sensors
	// We use oA2DX so we can bias the values up to permit single bit checking
	// --------------------------------
   	LeftLine.IOLine 		= IO_LINE_LEFT;
   	LeftLine.Center 		= ZERO_OFFSET;
   	LeftLine.Operate 		= cvTrue;

   	CenterLine.IOLine 		= IO_LINE_CENTER;
   	CenterLine.Center 		= ZERO_OFFSET;
   	CenterLine.Operate 		= cvTrue;

   	RightLine.IOLine 		= IO_LINE_RIGHT;
   	RightLine.Center 		= ZERO_OFFSET;
   	RightLine.Operate 		= cvTrue;

	// --------------------------------
   	// Setup the Servos	
   	// Note: The StartDrive() function must be called 
   	// before the servos will be activated.
	// --------------------------------
	
	// Set the servo Idle Position
   	LeftWheel.IOLine 		= IO_WHEEL_LEFT;
   	LeftWheel.Center 		= WHEEL_OFFSET;
   	LeftWheel.Value 		= WHEEL_STOP;
   	LeftWheel.Refresh 		= cvTrue;
   	LeftWheel.InvertOut		= 0;

   	RightWheel.IOLine 		= IO_WHEEL_RIGHT;
   	RightWheel.Center 		= WHEEL_OFFSET;
   	RightWheel.Value 		= WHEEL_STOP;
   	RightWheel.Refresh 		= cvTrue;
   	RightWheel.InvertOut	= 1;

	// --------------------------------
   	// Setup the Status LEDs (but disable them)	
	// --------------------------------
	LeftEyeOn.IOLine		= IO_DEBUG_LED_1 ;
	LeftLineWhite.IOLine	= IO_DEBUG_LED_2 ;
	CenterLineWhite.IOLine	= IO_DEBUG_LED_3 ;
	RightLineWhite.IOLine	= IO_DEBUG_LED_4 ;
	RightEyeOn.IOLine		= IO_DEBUG_LED_5 ;

	LeftEyeOn.Direction		  = cvInput ;
	LeftLineWhite.Direction	  = cvInput ;
	CenterLineWhite.Direction = cvInput ;
	RightLineWhite.Direction  = cvInput ;
	RightEyeOn.Direction	  = cvInput ;
}

//-----------------------------------------------------------------
//	SetupVC()
//	Configure all the Virtual Circuits
//-----------------------------------------------------------------
Void	SetupVC(Void)
{
	// --------------------------------
   	// setup the Eye Virtual Circuits
	// --------------------------------
	// Wire High Level to Line On.
	LeftEyeWire.Input.Link(LeftEye.Negative);
	LeftEyeWire.Output.Link(LeftEyeOn);
	LeftEyeWire.InvertIn 	= cvTrue;	// Transfer inverted value
	LeftEyeWire.Operate 	= cvTrue;

	// Wire High Level to Line On.
	RightEyeWire.Input.Link(RightEye.Negative);
	RightEyeWire.Output.Link(RightEyeOn);
	RightEyeWire.InvertIn 	= cvTrue;	// Transfer inverted value
	RightEyeWire.Operate 	= cvTrue;
	
	// ---------------------
   	// setup the Line Virtual Circuits
	// ---------------------
	// Wire Low Level to White
	LeftLineWire.Input.Link(LeftLine.Negative);
	LeftLineWire.Output.Link(LeftLineWhite);
	LeftLineWire.Operate 	= cvTrue;

	// Wire Low Level to White
	CenterLineWire.Input.Link(CenterLine.Negative);
	CenterLineWire.Output.Link(CenterLineWhite);
	CenterLineWire.Operate 	= cvTrue;

	// Wire Low Level to White
	RightLineWire.Input.Link(RightLine.Negative);
	RightLineWire.Output.Link(RightLineWhite);
	RightLineWire.Operate 	= cvTrue;
}

//-----------------------------------------------------------------
//	Drive(LeftSpeed, RightSpeed, DriveTime)
//	This function drives both wheels at the indicated speeds
//	The motion is held for "DriveTime" tenths of a second
//	No time limit is placed if Time = 0
//
//	Positive values are forward, Negative are backwards.
//	Max range is +/- 18
//
//-----------------------------------------------------------------
Void Drive(Byte LeftSpeed, Byte RightSpeed, Byte DriveTime)
{ 
	// Set left wheel speed (Disable if zero)
	If (LeftSpeed == 0)
	{
		LeftWheel.Value  	= WHEEL_STOP ;
	   	LeftWheel.Operate 	= cvFalse;
	}
	Else
	{
	   	LeftWheel.Operate 	= cvTrue;
		LeftWheel.Value  	= WHEEL_STOP + LeftSpeed ;
	}

	// Set right wheel speed (Disable if zero)
	If (RightSpeed == 0)
	{
		RightWheel.Value  	= WHEEL_STOP ;
	   	RightWheel.Operate 	= cvFalse;
	}
	Else
	{
	   	RightWheel.Operate 	= cvTrue;
		RightWheel.Value  	= WHEEL_STOP + RightSpeed ;
	}
	
	// Continue action for requested time period in tenths.
	If (DriveTime > 0)
	{
		ActionTimer	= DriveTime;
		While(ActionTimer.NonZero);
	}
}

//-----------------------------------------------------------------
//	EnableDiagnostics()
//	This function switches the LED I/O pins to outputs.
//  Call this function if you have LEDS wired to IO lines 24-28
//-----------------------------------------------------------------
Void EnableDiagnostics(Void)
{
	// Set the LEDs to Outputs
	LeftEyeOn.Direction			= cvOutput ;
	LeftLineWhite.Direction		= cvOutput ;
	CenterLineWhite.Direction	= cvOutput ;
	RightLineWhite.Direction	= cvOutput ;
	RightEyeOn.Direction		= cvOutput ;
}

//-----------------------------------------------------------------
//	FollowerCalibrate()
//	This function is used to measure the current reflected IR from
//	a White or Black Line by the Line sensors.
//	The threshold is set halfway between the inner and outer sensors
//	This call assumes the Bot is centered on the line.
//-----------------------------------------------------------------
Void FollowerCalibrate(Void)
{
	// Set all the lines back to zero offset
   	LeftLine.Center 		= ZERO_OFFSET;
   	CenterLine.Center 		= ZERO_OFFSET;
   	RightLine.Center 		= ZERO_OFFSET;

	// Read the two outer sensors & take average
	TempWord = (RightLine.Value + LeftLine.Value) >> 1 ;
	
	// Read the inner sensors & take average with outer average
	TempWord = (CenterLine.Value + TempWord) >> 1 ;
	
	// Set a new offset to make A "white line" negative.
   	LeftLine.Center 		= ZERO_OFFSET - TempWord;
   	CenterLine.Center 		= ZERO_OFFSET - TempWord;
   	RightLine.Center 		= ZERO_OFFSET - TempWord;
}

//-----------------------------------------------------------------
//	SumoCalibrate()
//	This function is used to measure the current reflected IR from
//	the SUMO ring by the Line sensors and determines a "White Line"
//	threshold by taking 3/4 of the current level.
//	This call assumes the Bot is NOT on a white line at the time 
//-----------------------------------------------------------------
Void SumoCalibrate(Void)
{
	// Set all the lines back to zero offset
   	LeftLine.Center 		= ZERO_OFFSET;
   	CenterLine.Center 		= ZERO_OFFSET;
   	RightLine.Center 		= ZERO_OFFSET;

	// Read each of the three line sensors and calculate a line level
	// Equal to 3/4 of the initial level
	TempWord = (RightLine.Value + CenterLine.Value + LeftLine.Value) >> 2; ;
	
	// Set a new offset to make A "white line" negative.
   	LeftLine.Center 		= ZERO_OFFSET - TempWord;
   	CenterLine.Center 		= ZERO_OFFSET - TempWord;
   	RightLine.Center 		= ZERO_OFFSET - TempWord;

}

//-----------------------------------------------------------------
//	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);
}