//-----------------------------------------------------------------
// Program:		PBot_BOB-G_VC6.osc
// Function:	Lesson G. Closed Loop Speed control
// Designer:	Phil Malone,  phil@philbot.com  www.philbot.com 
// Details:		http://www.philbot.com  
//
// Device:		Bot On Board (BOB) V1.1  
// Compiler:	OOPic 6.x
// Motor Drive:	SN75410
// Quad encoder:OPB610  
//
//  BOB is a bot built on top of the Tamiya Twin Motor Gearbox.
//
// Version	Date		Comment	
// -------	---------	------- 
//  1.00    8/16/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	

Const	TAMIYA_HIGH_GEAR	=  	0;		// The high and low gear ratios drive the 
Const	TAMIYA_LOW_GEAR		=  	1;		// wheels in opposite direction, so here is 
										// where we compensate.

Const	GEAR_SELECTION		= TAMIYA_LOW_GEAR;	// set to LOW_GEAR or HIGH_GEAR
Const	PID_RATE			= 185;		// Rate divider for 4Hz.	
Const	PID_LIMIT			=  75;		// Limit active Drive Signal
Const	MOTOR_DEAD_BAND		=  20;		// Minimum drive offset

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

// Wheel Optical Encoder Objects
oQencode LeftClicks   	= New oQencode;	// Value for Left Encoder
oQencode RightClicks   	= New oQencode;	// Value for Right Encoder

//-----------------------------------------------------------------
// Virtual Circuits.  Use these to manipulate BOB hardware.
//-----------------------------------------------------------------
oClock	PID_Clock		= New oClock;	// Set to run at PID Rate

oBusIC	SetSpeedLeft	= New oBusIC;	// Holds the desired clicks
oMath	PIDLeft			= New oMath;	// Updates the Drive Signal
oBusIC	DriveLeft		= New oBusIC;	// Holds the motor output
oCompare0 LimitLeft		= New oCompare0;	// Indicates out of limit condition 
oWire	LimitLeftWire	= New oWire;	// Transfers outoflimit condition 
 
oBusIC	SetSpeedRight	= New oBusIC;	// Holds the desired clicks
oMath	PIDRight		= New oMath;	// Updates the Drive Signal
oBusIC	DriveRight		= New oBusIC;	// Holds the motor output
oCompare0 LimitRight	= New oCompare0;	// Indicates out of limit condition 
oWire	LimitRightWire	= New oWire;	// Transfers outoflimit condition 

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

//#################################################################
//
// Now comes the standard functions that All lessons have
//
//#################################################################

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

	// Do a range of motions
	SetSpeed(  10,   10,  100);	// Straight forward real slow for 10 seconds
	SetSpeed(  30,  -30,   20); // Spin right for 2 seconds
	SetSpeed( -80,  -80,  100);	// Back up real fast for 10 seconds
	SetSpeed(   0,   0,   0);	// Stop;
	
	While(1);					// Wait.
	
}

//-----------------------------------------------------------------
//	Required Function
//	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 Wheel Encoders
	// --------------------------------
	LeftClicks.IOLine1	= IO_ENCODER_LEFT_FRONT ;
	LeftClicks.IOLine2	= IO_ENCODER_LEFT_BACK ;
	LeftClicks.InvertD	= GEAR_SELECTION ;
	LeftClicks.Operate	= cvTrue;

	RightClicks.IOLine1	= IO_ENCODER_RIGHT_FRONT ;
	RightClicks.IOLine2	= IO_ENCODER_RIGHT_BACK ;
	RightClicks.InvertD	= GEAR_SELECTION ;
	RightClicks.Operate	= cvTrue;

}

//-----------------------------------------------------------------
//	SetupVC()
//	Configure all the Virtual Circuits here
//-----------------------------------------------------------------
Void	SetupVC( Void )
{
	// PID Clocks
	PID_Clock.Mode			= 1;			// Pulse High
	PID_Clock.Rate			= PID_RATE;		// Desired divider
	PID_Clock.Operate		= cvTrue;		// Run the PID clock

	// Left Wheel PID
	SetSpeedLeft.ClockIn.Link(PID_Clock);  	// Use PID Clock
	SetSpeedLeft.InvertC	= cvTrue;		// Transfer on negative clock edge
	SetSpeedLeft.Output.Link(LeftClicks);	// Clock Data into Encoder
	SetSpeedLeft.Operate	= cvTrue;		// Enable VC

	PIDLeft.Input1.Link(LeftClicks);		// Read Encoder Value
	PIDLeft.Input2.Link(LeftMotor);			// Read Motor Value
	PIDLeft.Output.Link(DriveLeft);			// Send Sum to Potential Drive Value
	PIDLeft.Mode			= cvAdd;		// Add the inputs
	PIDLeft.Operate			= cvTrue;		// Enable VC

	LimitLeft.Input.Link(DriveLeft);		// Test Potential Drive Signal
	LimitLeft.Fuzziness		= PID_LIMIT;	// Set invalid width
	LimitLeft.Operate		= cvTrue;		// Enable VC

	DriveLeft.ClockIn.Link(PID_Clock);		// Use PID Clock
	DriveLeft.Output.Link(LeftMotor);		// Clock Data into Motor

	LimitLeftWire.Input.Link(LimitLeft.Between);   // Transfer "Between" to enable Drive
	LimitLeftWire.Output.Link(DriveLeft.Operate);	
	LimitLeftWire.Operate	= cvTrue;		// Enabled


	// Right Wheel PID
	SetSpeedRight.ClockIn.Link(PID_Clock);  // Use PID Clock
	SetSpeedRight.InvertC	= cvTrue;		// Transfer on negative clock edge
	SetSpeedRight.Output.Link(RightClicks);	// Clock Data into Encoder
	SetSpeedRight.Operate	= cvTrue;		// Enable VC

	PIDRight.Input1.Link(RightClicks);		// Read Encoder Value
	PIDRight.Input2.Link(RightMotor);		// Read Motor Value
	PIDRight.Output.Link(DriveRight);		// Send Sum to Potential Drive Value
	PIDRight.Mode			= cvAdd;		// Add the inputs
	PIDRight.Operate		= cvTrue;		// Enable VC

	LimitRight.Input.Link(DriveRight);		// Test Potential Drive Signal
	LimitRight.Fuzziness	= PID_LIMIT;	// Set invalid width
	LimitRight.Operate		= cvTrue;		// Enable VC

	DriveRight.ClockIn.Link(PID_Clock);		// Use PID Clock
	DriveRight.Output.Link(RightMotor);		// Clock Data into Motor

	LimitRightWire.Input.Link(LimitRight.Between);   // Transfer "Between" to enable Drive
	LimitRightWire.Output.Link(DriveRight.Operate);	
	LimitRightWire.Operate	= cvTrue;		// Enabled
}


//#################################################################
//
// Now comes the functions specific to this lesson
//
//#################################################################


//-----------------------------------------------------------------
//	Set Speed(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 +/- 128
//	Time Range is 0 - 255
//-----------------------------------------------------------------
Void SetSpeed(Byte LeftSpeed, Byte RightSpeed, Byte DriveTime)
{ 
	// Set Left Speed
	If (LeftSpeed != 0)
	{
		// Turn off brake
		LeftMotor.Brake = cvOff;		
		
		// Do Setup based in direction of motion.
		If (LeftSpeed > 128)  // Reverse speed
		{
			// Move speed range to (-1 to -9) and set initial drive signal
			LeftSpeed 		= ((255 - LeftSpeed) >> 4) + 1;
			LeftMotor.Value = - ((LeftSpeed << 2) + MOTOR_DEAD_BAND) ;
			SetSpeedLeft 	= - LeftSpeed ;
		}
		Else
		{
			// Move speed range to (1 to 9) and set initial drive signal
			LeftSpeed = (LeftSpeed >> 4) + 1;
			LeftMotor.Value = (LeftSpeed << 2) + MOTOR_DEAD_BAND ;
			SetSpeedLeft 	= LeftSpeed ;
		}
	}
	Else
	{
		// Stop motor and feedback
		LeftMotor.Value	= 0 ;
		SetSpeedLeft 	= 0 ;
		LeftMotor.Brake = cvOn;		
	}

	// Set Right Speed
	If (RightSpeed != 0)
	{
		// Turn off brake
		RightMotor.Brake = cvOff;		

		// Do Setup based in direction of motion.
		If (RightSpeed > 128)  // Reverse speed
		{
			// Move speed range to (-1 to -9) and set initial drive signal
			RightSpeed 		 = ((255 - RightSpeed) >> 4) + 1;
			RightMotor.Value = - ((RightSpeed << 2) + MOTOR_DEAD_BAND) ;
			SetSpeedRight 	 = - RightSpeed ;
		}
		Else
		{
			// Move speed range to (1 to 9) and set initial drive signal
			RightSpeed = (RightSpeed >> 4) + 1;
			RightMotor.Value = (RightSpeed << 2) + MOTOR_DEAD_BAND ;
			SetSpeedRight 	 = RightSpeed ;
		}
	}
	Else
	{
		// Stop motor and feedback
		RightMotor.Value = 0 ;
		SetSpeedRight 	 = 0 ;
		RightMotor.Brake = cvOn;		
	}


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

//-----------------------------------------------------------------
//	Stop()
//	This function turns off the motor drive
//
//-----------------------------------------------------------------
Void Stop(Void)
{ 
	SetSpeed(0, 0, 0)	
}