//-----------------------------------------------------------------
// oUserClass Object: "PBot_MkIII_VC" (Virtual Circuit)
//
// Designer:	Phil Malone,  phil@philbot.com  www.philbot.com
// Details:	http://www.philbot.com/software_OOPic_MkIII_VC.htm
//
// 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.  The class is used to 
// set up all the I/O channels and provide boolean flags to 
// indicate when a each sensor is detecting its target.
//
//  Subroutones are provided to calibrate the inputs and set the 
// MarkIII in motion in several ways.
//
//  PBotMkIII_VC does NOT decide what actions the Sumo should take 
// in response to changing sensor inputs, this must be done by the 
// calling program.  This way, many different figting approaches
// may be tried.
//
//  To help debugging, 6 LEDs may be attached to I/O Bits 24-29.
//   Connect the Leds to +0V via a series 200 ohm Resistor
//
//  This oUserClass is written for OOPIC Compiler V5.0 and V6.0	
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00     	5/12/2005	First Virtual Circuit release
//  1.01     	5/17/2005	More numbers changed to Const
//
//-----------------------------------------------------------------

//-----------------------------------------------------------------
// PUBLIC Objects
//-----------------------------------------------------------------

// Object constants, used for default values
Const   WHEEL_OFFSET	= 18;	   // Count for Full Reverse
Const   WHEEL_STOP	= 36;	   // Null position for servo

Const   LINE_LEVEL	= 32;	   // Default Line black/white level.

Const	EYE_OFFSET	= -100 ;   // Bias the Eye level up by 28	
Const   EYE_DIFFERENCE	= 24;	   // Set diff less than Eye Bias
Const   EYE_LEVEL	= 52;	   // Level for "See Him", includes Bias

Const	LEFT_EYE_IO	= 4;
Const	RIGHT_EYE_IO	= 3;
Const	LEFT_LINE_IO	= 7;
Const	CENTER_LINE_IO	= 6;
Const	RIGHT_LINE_IO	= 5;

Const	LEFT_WHEEL_IO	= 10;
Const	RIGHT_WHEEL_IO	= 9;

Const	DEBUG_LED_1	= 24;
Const	DEBUG_LED_2	= 25;
Const	DEBUG_LED_3	= 26;
Const	DEBUG_LED_4	= 27;
Const	DEBUG_LED_5	= 28;
Const	DEBUG_LED_6	= 29;

// Object Devices
oA2DX	LeftEye   	= New oA2DX;
oA2DX	RightEye   	= New oA2DX;

oA2D	LeftLine  	= New oA2D;
oA2D	CenterLine  	= New oA2D;
oA2D	RightLine  	= New oA2D;

oDIO1	LeftEyeOn	= New oDIO1;
oDIO1	LeftLineOn	= New oDIO1;
oDIO1	CenterLineOn	= New oDIO1;
oDIO1	RightLineOn	= New oDIO1;
oDIO1	RightEyeOn	= New oDIO1;
oDIO1	BothEyesOn	= New oDIO1;

oServo	LeftWheel 	= New oServo;
oServo	RightWheel	= New oServo;

oByte	LineLevel	= New oByte;
//-----------------------------------------------------------------
// Public Variables
//-----------------------------------------------------------------

Byte		WheelStop;
Word		TempWord;

// Virtual Circuit objects
oCompare  	LeftLineComp	= New oCompare;
oCompare  	CenterLineComp	= New oCompare;
oCompare  	RightLineComp	= New oCompare;

oCompare  	EyesComp	= New oCompare;
oCompare0  	EyeComp		= New oCompare0;
oGate		BothEyes	= New oGate(2);

oWire	  	LeftEyeWire	= New oWire;
oWire	  	LeftLineWire	= New oWire;
oWire	  	CenterLineWire	= New oWire;
oWire	  	RightLineWire	= New oWire;
oWire	  	RightEyeWire	= New oWire;

//-----------------------------------------------------------------
//	MarkIII User Defined Object Constructor
//-----------------------------------------------------------------
Sub Void Main(Void)
{
	// --------------------------------
   	// setup the Eye sensors
	// --------------------------------

	// Configure I/O channels and enable inputs
	// We need to use oA2DX so we can bias the values up to permit 
	// the oCompare object to apply the "fuzzyness" factor without going negative
   	LeftEye.IOLine 		= LEFT_EYE_IO;
   	LeftEye.Center		= EYE_OFFSET;	// Bias the level up
   	LeftEye.Unsigned 	= cvTrue;	// Use whole 0-255 span
   	LeftEye.Limit		= cvFalse;	// Do not limit range to 127
   	LeftEye.Operate 	= cvTrue;

   	RightEye.IOLine 	= RIGHT_EYE_IO;
   	RightEye.Center		= EYE_OFFSET;	// Bias the level up
   	RightEye.Unsigned 	= cvTrue;	// Use whole 0-255 span
   	RightEye.Limit		= cvFalse;	// Do not limit range to 127
   	RightEye.Operate 	= cvTrue;

	// --------------------------------
   	// setup the line sensors
	// --------------------------------
	
	// Start with a default Line Level for threshold.
	// This is changed by CalibrateLine() function
   	LineLevel 		= LINE_LEVEL;

	// Configure I/O channels and enable inputs
   	LeftLine.IOLine 	= LEFT_LINE_IO;
   	LeftLine.Operate 	= cvTrue;

   	CenterLine.IOLine 	= CENTER_LINE_IO;
   	CenterLine.Operate 	= cvTrue;

   	RightLine.IOLine 	= RIGHT_LINE_IO;
   	RightLine.Operate 	= cvTrue;

	// --------------------------------
   	// Setup the Servos	
   	// Note: The StartDrive() function must be called 
   	// before the servos will be activated.
	// --------------------------------
	
	// Set the servo Idle Position
   	WheelStop   		= WHEEL_STOP ;

   	LeftWheel.Operate	= cvFalse;
   	LeftWheel.Refresh 	= cvTrue;
   	LeftWheel.IOLine 	= LEFT_WHEEL_IO;
   	LeftWheel.InvertOut	= 0;
   	LeftWheel.Center 	= WHEEL_OFFSET;
   	LeftWheel.Value 	= WheelStop;

   	RightWheel.Operate	= cvFalse;
   	RightWheel.Refresh 	= cvTrue;
   	RightWheel.IOLine 	= RIGHT_WHEEL_IO;
   	RightWheel.InvertOut	= 1;
   	RightWheel.Center 	= WHEEL_OFFSET;
   	RightWheel.Value 	= WheelStop;

	// --------------------------------
   	// Setup the Status LEDs	
	// --------------------------------
	LeftLineOn.IOLine	= DEBUG_LED_1 ;
	LeftLineOn.Direction	= cvOutput ;
	LeftEyeOn.IOLine	= DEBUG_LED_2 ;
	LeftEyeOn.Direction	= cvOutput ;
	CenterLineOn.IOLine	= DEBUG_LED_3 ;
	CenterLineOn.Direction	= cvOutput ;
	RightEyeOn.IOLine	= DEBUG_LED_4 ;
	RightEyeOn.Direction	= cvOutput ;
	RightLineOn.IOLine	= DEBUG_LED_5 ;
	RightLineOn.Direction	= cvOutput ;
	BothEyesOn.IOLine	= DEBUG_LED_6 ;
	BothEyesOn.Direction	= cvOutput ;

	// --------------------------------
   	// setup the Eye Virtual Circuits
	// --------------------------------
	
	// Compare left and right eyes.
	EyesComp.Input.Link(LeftEye);
	EyesComp.ReferenceIn.Link(RightEye);
	EyesComp.Fuzziness 	= EYE_DIFFERENCE;
	EyesComp.Operate	= cvTrue;

	// Compare Righ Eye with threshold
	EyeComp.Input.Link(RightEye);
	EyeComp.Fuzziness 	= EYE_LEVEL;
	EyeComp.Operate 	= cvTrue;

	// Wire Left Eye High to Left Eye On
	LeftEyeWire.Input.Link(EyesComp.Above);
	LeftEyeWire.Output.Link(LeftEyeOn);
	LeftEyeWire.Operate	= cvTrue;

	// Wire Right Eye High to Right Eye On
	RightEyeWire.Input.Link(EyesComp.Below);
	RightEyeWire.Output.Link(RightEyeOn);
	RightEyeWire.Operate	= cvTrue;

	// Look for both eyes active:
	// AND "no eye domminance" with "high levels"
	BothEyes.Input1.Link(EyesComp.Between);
	BothEyes.Input2.Link(EyeComp.Above);
	BothEyes.Output.Link(BothEyesOn);
	BothEyes.InvertIn1 	= cvTrue;
	BothEyes.InvertIn2 	= cvTrue;
	BothEyes.InvertOut 	= cvTrue;
	BothEyes.Operate	= cvTrue;

	// ---------------------
   	// setup the Line Virtual Circuits
	// ---------------------

	// Compare Line level to threshold
	LeftLineComp.Input.Link(LeftLine);
	LeftLineComp.ReferenceIn.Link(LineLevel);
	LeftLineComp.Fuzziness 	= 0;

	// Wire Low Level to Line On.
	LeftLineWire.Input.Link(LeftLineComp.Below);
	LeftLineWire.Output.Link(LeftLineOn);

	// Compare Line level to threshold
	CenterLineComp.Input.Link(CenterLine);
	CenterLineComp.ReferenceIn.Link(LineLevel);
	CenterLineComp.Fuzziness= 0;

	// Wire Low Level to Line On.
	CenterLineWire.Input.Link(CenterLineComp.Below);
	CenterLineWire.Output.Link(CenterLineOn);

	// Compare Line level to threshold
	RightLineComp.Input.Link(RightLine);
	RightLineComp.ReferenceIn.Link(LineLevel);
	RightLineComp.Fuzziness	= 0;

	// Wire Low Level to Line On.
	RightLineWire.Input.Link(RightLineComp.Below);
	RightLineWire.Output.Link(RightLineOn);

	// --------------------------------
   	// Enable all the Virtual Circuits
	// --------------------------------
	
	LeftLineComp.Operate 	= cvTrue;
	LeftLineWire.Operate 	= cvTrue;
	CenterLineComp.Operate 	= cvTrue;
	CenterLineWire.Operate 	= cvTrue;
	RightLineComp.Operate 	= cvTrue;
	RightLineWire.Operate 	= cvTrue;
}

//-----------------------------------------------------------------
//	StartServos()
//	This function MUST be called to activate the Servo Outputs
//-----------------------------------------------------------------
Sub Void StartServos( Void )
{ 
    	LeftWheel.Operate = cvTrue;
	RightWheel.Operate = cvTrue;
}


//-----------------------------------------------------------------
//	Forward(Speed)
//	This function drives both wheels at the same speed.
//
//	Param: Speed is applied to both wheels
//	Positive values are forward, Negative are backwards.
//	Max range is +/- 18
//-----------------------------------------------------------------
Sub Void Forward(Byte Speed)
{ 
	LeftWheel.Value = WheelStop + Speed  ;
    	RightWheel.Value = WheelStop + Speed  ;
}

//-----------------------------------------------------------------
//	Drive(Speed, Right)
//	This function drives both wheels at different speeds
//	The net result is an Arcing motion
//
//	Param: Speed is applied to both wheels
//	Positive values are forward, Negative are backwards.
//	Max range is +/- 18
//
//	Param: Abs value of Right is added to only one wheel.
//	If Right is Positive, Left wheel goes faster (Bot turns Right)
//	If Right is Negative, Right wheel goes faster (Bot turns Left)
//	Max range is +/- 18
//
//-----------------------------------------------------------------
Sub Void Drive(Byte DSpeed, Byte DTurn)
{ 
	If (DTurn < 128)
	{
		LeftWheel.Value = WheelStop + DSpeed + DTurn ;
  		RightWheel.Value = WheelStop + DSpeed ;
	}
	Else
	{
		LeftWheel.Value = WheelStop + DSpeed ;
   		RightWheel.Value = WheelStop + DSpeed - DTurn ;
	}
}

//-----------------------------------------------------------------
//	Turn(Right)
//	This function drives one wheel while leaving the other one stopped
//	The net result is a rotation around the stopped wheel
//
//	Param: Abs value of Right is added to only one wheel.
//	If Right is Positive, Left wheel turns forward (Bot turns Right)
//	If Right is Negative, Right wheel turns forward (Bot turns Left)
//	Max range is +/- 18
//-----------------------------------------------------------------
Sub Void Turn(Byte TRight)
{ 
	If (TRight < 128)
	{
		LeftWheel.Value = WheelStop + TRight  ;
    		RightWheel.Value = WheelStop ;
	}
	Else
	{
		LeftWheel.Value = WheelStop ;
    		RightWheel.Value = WheelStop - TRight  ;
	}
}

//-----------------------------------------------------------------
//	Spin(Right)
//	This function both wheels in opposite directions
//	The net result is a rotation around the center of the robot
//
//	Param: Right is added to left wheel, subtracted from right.
//	If Right is Positive, Bot spins Right
//	If Right is Negative, Bot spins Left
//	Max range is +/- 18
//-----------------------------------------------------------------
Sub Void Spin(Byte SRight)
{ 
	LeftWheel.Value = WheelStop + SRight  ;
    	RightWheel.Value = WheelStop - SRight  ;
}

//-----------------------------------------------------------------
//	CalibrateLine()
//	This function is used to measure the current reflected IR from
//	the 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 
//-----------------------------------------------------------------
Sub Void CalibrateLine(Void)
{
	// Read each of the three line sensors and set a line level
	// Equal to 3/4 of the initial level

	TempWord = RightLine.Value + CenterLine.Value + LeftLine.Value ;
	LineLevel = TempWord / 4 ;
}