//-----------------------------------------------------------------
// Program:		PBot_BOB-C_VC6.osc
// Function:	Lesson C. Motor Driver Test
// 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
// 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	OBSTACLE			=  cvLow; 	// Eye Sees Obstacle	
Const	CLEAR				=  cvHigh;	// Eye Does Not see Obstacle	

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

// 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

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

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

	// Run Basic Wheel Tests
	Drive( 50,  0,  20);		// Left forward for 2 seconds
	Drive(-50,  0,  20);		// Left Reverse for 2 seconds
	Drive(  0, 50,  20);		// Right forward for 2 seconds
	Drive(  0,-50,  20);		// Right Reverse for 2 seconds

	Drive( 100,-100, 10);		// Fast spim to Right for 1 second
	Drive(-100, 100, 10);		// Fast spim to Left  for 1 second

	// Stop and wait two seconds then start forward
	Brake();		
	WaitTenths(20);
	Drive( 50, 50, 10);			// Forward
	
	// Run forever finding and avoiding walls
	While (cvTrue)
	{
		// Go forward if the path is clear
		If (LeftEye == CLEAR & RightEye == CLEAR)		
		{
			Drive(60, 60, 0);			
		}
		
		// Reverse and spin if the path is blocked in both eyes
		Else If (LeftEye == OBSTACLE & RightEye == OBSTACLE)
		{
			Brake();
			Drive(-50, -50, 5);			
			Drive( 50, -50, 6);			
		}
		
		// Spin to the right if the left is blocked
		Else If (LeftEye == OBSTACLE)
		{
			Drive( 50, -50, 0);			
		}

		// Spin to the left if the right is blocked
		Else If (RightEye == OBSTACLE)
		{
			Drive(-50,  50, 0);			
		}
	}
}

//#################################################################
// 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 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
//#################################################################

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