// Working 
// by Phil Winder || www.philwinder.com

// DESCRIPTION: **************************************************
// This code provides functions to properly operate the motor.
// Care must be taken when messing with the duty and the period 
// values for the PWM.  It is easy to get confused.
// Also bear in mind that if you want to use the driver for one
// motor (ie. forwards and backwards) then make sure to take the
// fact that the duty would be the opposite if direction was 
// changed.  Eg. If the motor was moving forward with a 75% duty
// then if the direction was changed, the duty would become 25%.

// NOTES: ********************************************************
// # Need to have a Cap on the motor driver or the current sink 
//   resets the PIC.

// VERSION INFORMATION: ******************************************
// V1.0 @ 10/05/07 - First version. Code taken from inital test program
// ***************************************************************

#include <delays.h>
#include <pwm.h> 
#include <timers.h> 

// Defines
//#define DEBUG

// Function Definitions
void Init_PWM(void);
void Move_Forward(unsigned char);
void Move_Right(unsigned char);
void Move_Left(unsigned char);
void Stop(void);

//********************************************************************
//Name: Move Forward
//Usage: Move_Forward(unsigned char Speed)
//	Where: Speed = duty cycle in %
//Description: 	Moves the motors forward with a certain speed.
//********************************************************************
void Move_Forward(unsigned char Speed)
{
	unsigned long Speedlong;

#ifdef DEBUG
	LATA = 3;
#endif

	Speedlong = (unsigned long) (Speed*4.424);
	SetDCPWM1(Speedlong);	// 212 = 10kHz i.e. 50% Duty.
	SetDCPWM2(Speedlong);	// 212 = 10kHz i.e. 50% Duty.
}

//********************************************************************
//Name: Move Right
//Usage: Move_Right(unsigned char Speed)
//	Where: Speed = duty cycle in %
//Description: 	Moves the bot right with a certain speed.
//********************************************************************
void Move_Right(unsigned char Speed)
{
	unsigned long Speedlong;

#ifdef DEBUG
	LATA = 2;
#endif

	Speedlong = (unsigned long) (Speed*4.424);
	SetDCPWM1(0);			// 0 = Stop
	SetDCPWM2(Speedlong);	// 212 = 10kHz i.e. 50% Duty.		
}

//********************************************************************
//Name: Move Left
//Usage: Move_Left(unsigned char Speed)
//	Where: Speed = duty cycle in %
//Description: 	Moves the bot Left with a certain speed.
//********************************************************************
void Move_Left(unsigned char Speed)
{
	unsigned long Speedlong;

#ifdef DEBUG
	LATA = 1;
#endif

	Speedlong = (unsigned long) (Speed*4.424);
	SetDCPWM1(Speedlong);	// 212 = 10kHz i.e. 50% Duty.		
	SetDCPWM2(0);			// 0 = Stop
}

//********************************************************************
//Name: Stop
//Usage: Stop();
//Description: 	Stops the Bot dead.
//********************************************************************
void Stop(void)
{
#ifdef DEBUG
	LATA = 0;
#endif

	SetDCPWM1(0);	// 0 = Stop
	SetDCPWM2(0);	// 0 = Stop
}

//********************************************************************
//Name: Initialise PWM Module
//Usage: Init_PWM();
//Description: 	Initialises the PWM module for operation and sets
//				the duty period to 0 (off).
//********************************************************************
void Init_PWM(void)		// Initalise the PWM module.
{
	// Timer Pre/Postscale of 1
	OpenTimer2 (TIMER_INT_OFF & T2_PS_1_1 & T2_POST_1_1); 
	OpenPWM1 (98); 		// 98 = 5kHz
	SetDCPWM1 (0);		// Set PWM off
	OpenPWM2 (98); 		// 98 = 5kHz
	SetDCPWM2 (0);		// Set PWM off
}