//-----------------------------------------------------------------
// Program:		PBot_BOB-D_VC6.osc
// Function:	Lesson D. SUMO BOB
// 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/8/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	SEEK_TIMER			=  50;		// End seek after 5 seconds

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

// LED Object (accessed as group of 4 bits)
oDIO4 		LEDs	   	= New oDIO4; 		// Driver for LEDs

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

// Digital Proximity sensors
oDIO1	  LeftEye		= New oDIO1;  	 // Left Prox Sensor
oDIO1	  RightEye		= New oDIO1;  	 // Right Prox Sensor

// Analog Line Sensors
oA2DX	  LeftLine		= New oA2DX;  	 // Left Line Sensor
oA2DX	  RightLine		= New oA2DX;  	 // Right Line Sensor


//-----------------------------------------------------------------
// General Variables
//-----------------------------------------------------------------

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

	// Assume that BOB is on black surface by now.  Calibrate sensors
	CalibrateLineSensors();

	// Run SUMO rules.
	While (cvTrue)
	{
		// Run down the SUMO priority list

		// Priority 1: Avoid the border
		If (LeftLine.Negative)
		{
			// Set debug Status
			LEDs.Value = 1;
			
			// If we see the left line, backup & start Slow Spin Right
			Drive(-80, -80,  5); 
			Brake();
			Drive(40, -40, 0); 

			// Set timeout in case we get stuck seeking
			ActionTimer = SEEK_TIMER ;
		}
		Else If (RightLine.Negative)
		{
			// Set debug Status
			LEDs.Value = 1;

			// If we see right line, backup & start Slow Spin Left
			Drive(-80, -80,  5); 
			Brake();
			Drive(-40,	40,	   0); 

			// Set timeout in case we get stuck seeking
			ActionTimer = SEEK_TIMER ;
		}
		
		// Priority 2: Push the opponent
		Else If ((LeftEye == OPPONENT) & (RightEye == OPPONENT))
		{
			// Go forward
			Drive(100, 100,  0); 

			// Set debug Status
			LEDs.Value = 2;
		}

		// Priority 3: Turn towards the opponent
		Else If (LeftEye == OPPONENT)
		{
			// Turn left towards the target
			Drive(20, 100,  0); 

			// Set debug Status
			LEDs.Value = 4;
		}

		Else If (RightEye == OPPONENT)
		{
			// Turn right towards the target
			Drive(100, 20,  0); 

			// Set debug Status
			LEDs.Value = 4;
		}


		// Priority 4: Move forward If seek timeout has expired. 
		Else
		{
			// Has Seek timer reached zero?
			If (ActionTimer.Value == 0)
			{
				// Go forward
				Drive(70, 70,  0); 

				// Set debug Status
				LEDs.Value = 0;
			}
			Else
			{
				// Set debug Status
				LEDs.Value = 8;
			}
		}
	}
}

//#################################################################
// 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 Line Sensors
	// --------------------------------
	LeftLine.IOLine		= IO_LINE_LEFT;
	LeftLine.Center		= 127;
	LeftLine.Operate	= cvTrue;
	RightLine.IOLine	= IO_LINE_RIGHT;
	RightLine.Center	= 127;
	RightLine.Operate	= cvTrue;

	// --------------------------------
   	// setup the LED Drivers
	// --------------------------------
	LEDs.IOGroup			= IO_LED_GROUP ;
	LEDs.Nibble				= IO_LED_NIBBLE ;
	LEDs.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;
	
	// --------------------------------
   	// setup the Eye proximity sensors
	// --------------------------------
	LeftEye.IOLine		= IO_PROXIMITY_LEFT;
	LeftEye.Direction	= cvInput;	
	RightEye.IOLine		= IO_PROXIMITY_RIGHT;
	RightEye.Direction	= cvInput;	

}

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

//#################################################################
// Lesson specific functions
//#################################################################

//-----------------------------------------------------------------
//	CalibrateLineSensors()
//	This function sets the line sensor centers to the line threshold
//	A white surface will then have a negative sensor value
//	A black surface will then have a positive sensor value
//-----------------------------------------------------------------
Void CalibrateLineSensors(Void)
{
	Byte	Level;	 

	// Read the two line sensors and determine a line threshold
	// at 1/2 of the dark level (>>2 = divide by 4)
	Level = (LeftLine.Value + RightLine.Value) >> 2 ;
	
	// Set the offset such that levels below the threshold will appear negative
	LeftLine.Center		= 127 - Level;
	RightLine.Center	= 127 - Level;
}

//-----------------------------------------------------------------
//	Brake()
//	This function reduces the motor speed for 1/10th of a second
//	Then the wheels are set to zero
//-----------------------------------------------------------------
Void Brake(Void)
{
	// Reverse motor speed
	LeftMotor.Value		= 0 - LeftMotor;		
	RightMotor.Value	= 0 - RightMotor;		
	
	// Wait one tenth
	ActionTimer		= 1;
	While(ActionTimer.NonZero);

	LeftMotor.Value		= 0;		
	RightMotor.Value	= 0;		

	LeftMotor.Brake		= cvOn;		
	RightMotor.Brake	= cvOn;		
}

//-----------------------------------------------------------------
//	Drive(LeftSpeed, RightSpeed, Time)
//	This function drives both wheels at the indicated speed.
//	The motion is held for "Time" tenth of a second
//	No time limit is placed if Time = 0
//
//	Speed range is +/- 127
//	Time Range is 0 - 255
//-----------------------------------------------------------------
Void Drive(Byte LeftSpeed, Byte RightSpeed, Byte DriveTime)
{ 
	LeftMotor.Brake		= cvOff;		
	RightMotor.Brake	= cvOff;		

	LeftMotor.Value		= LeftSpeed  ;
   	RightMotor.Value	= RightSpeed  ;

	// Continue action for requested time period in tenths.
	If (DriveTime > 0)
	{
		ActionTimer	= DriveTime;
		While(ActionTimer.NonZero);
	}
}