//---------------------------------------------------------------------
//							 Software License Agreement
//
// The software supplied herewith by Microchip Technology Incorporated 
// (the Company) for its PICmicro Microcontroller is intended and 
// supplied to you, the Companys customer, for use solely and 
// exclusively on Microchip PICmicro Microcontroller products. The 
// software is owned by the Company and/or its supplier, and is 
// protected under applicable copyright laws. All rights are reserved. 
//  Any use in violation of the foregoing restrictions may subject the 
// user to criminal sanctions under applicable laws, as well as to 
// civil liability for the breach of the terms and conditions of this 
// license.
//
// THIS SOFTWARE IS PROVIDED IN AN AS IS CONDITION. NO WARRANTIES, 
// WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED 
// TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
// PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE COMPANY SHALL NOT, 
// IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR 
// CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
//
//---------------------------------------------------------------------
//
//		17motor.c
//
//		Written By:		Stephen Bowling, Microchip Technology
//		
//		This source code demonstrates the use of the PIC17C756A in a 
//		brush-DC servomotor application and is written for the MPLAB-C17
//		compiler v. 2.21.  The following files should be included in the  
//		project, which is compiled for the large memory model:
//
//		17motor.c		--		source file
//		c0l17.o			--		startup code
//		idata17.o		--		initialized data support
//		p17c756.o		--		processor definition module
//		int756l.o		--		interrupt handler routines
//		pmc756l.lib		--		library functions
//		p17c756l.lkr	--		linker script
//
//---------------------------------------------------------------------

#include <p17c756.h>         
#include <stdlib.h>
#include <usart16.h>
#include <string.h>
#include <timers16.h>
#include <captur16.h>
#include <pwm16.h>
#include <ctype.h>
#include <delays.h>
#include <mem.h>

#define  F  1
#define  W  0

const rom char start[] = "\r\n\r\n17C756A DC Servomotor";
const rom char ready[] = "\n\rREADY>";
const rom char error[] = "\n\rERROR!";

char inpbuf[8];							// input buffer for ASCII commands
char data[9];								// buffer for ASCII conversions
char command;								// holds the last parameter change
												// command that was received

unsigned char 
i,												// index to ASCII buffer
udata, 										// received character from USART
mode, 										// determines servo mode
tempchar, 
PRODHtemp,									// temp context saving for ISR
PRODLtemp,									// "
FSR0temp,									// "
FSR1temp;									// "
												
struct {										// holds status bits for servo
    unsigned            phase:1;		// first half/ second half of profile
    unsigned         neg_move:1;		// backwards relative move
    unsigned move_in_progress:1;		//
    unsigned 		  saturated:1;		// servo output is saturated
    unsigned         	 bit4:1;
    unsigned 				 bit5:1;
    unsigned             bit6:1;
    unsigned             bit7:1;
} stat ;

int

tempint3,									//
tempint2,									//
tempint1,									//
tempint0,									//
UpCount,										// encoder up counts during sample period
DnCount,										// encoder down counts "      "      "
u0,u1,										// current and previous position error
kp,ki,kd,									// PID gain constants
integral,									// PID error accumulation
ypwm,											// duty cycle derived from PID calculation
multcnd,multplr,							// holds values to be multiplied in mult()
velcom,vlim;								// commanded velocity, velocity limit

unsigned int accel;						// acceleration parameter for motion profile

union LONG
{
unsigned int ui[2];
int i[2];
char b[4];
};

union LONG
aarg,                      			// Used for math calculations.
barg,                      			//              "
ypid,                      			// Used to hold result of the PID calculations.
position,                  			// Commanded position.
mposition,                 			// Actual measured position.
fposition,									// Originally commanded position.
poserror,                  			// 32-bit position error calculated in the PID
mvelocity,                 			// measured velocity
velact,										// current commanded velocity
phase1dist,                			// total distance for first half of move.
flatcount;                 			// Holds the number of sample periods for which 
												// the velocity limit was reached in the first 
												// half of the move.  


//  Function Declarations----------------------------------------------

void main(void);              		// Required for the main function
void InitPorts(void);					// Initializes ports/peripherals
void InitVars(void);						// Initializes variable used in program
void DoCommand(void);					// Parses input buffer after a <CR> was received 
void ServoISR(void);						// Performs the error calculations and PID 
void UpdatePosition(void);				// Updates the measured motor position
void UpdateTrajectory(void);			// Does the velocity profile
void add32(void);							// Performs a 32 bit addition
void sub32(void);							// Performs a 32 bit subtraction
void mult(void);							// Performs a 16 x 16 --> 32 multiplication
void ulitoa(unsigned int value1, 	//
unsigned int value0, char *string);	//
char ntoh(unsigned int value);		//


//---------------------------------------------------------------------



void main(void)
{
InitVars();
InitPorts();
Install_PIV(ServoISR);					// Servo_ISR is installed as the peripheral
Enable();									// int. handler.  

putrsUSART1(start);
putrsUSART1(ready);
												          
while(1)                    			// This is the main program loop
    {											// that polls USART1 for received
      										// characters.
    if(PIR1bits.RC1IF)
   	{
   	switch(udata = ReadUSART1())
	      {
	      case 0x0d:  DoCommand();			// got a <CR>, so process the string
	                  strset(inpbuf, 0);	// clear the input buffer
	                  i = 0;					// clear the input buffer index
	                  putrsUSART1(ready);	// put a ready prompt on the screen
	                  break;
	                  
	      default:    inpbuf[i] = udata;	// put the received character in the
	                  i++;						// next buffer location and increment
	                  if(i > 7)				// the buffer index
	                     {			
	                     putrsUSART1(ready); 	// if we got more than 7 chars before a
	                     strset(inpbuf, 0);	// <CR>, clear the input buffer and clear
	                     i = 0;					// the buffer index
	                     }
	                  else putcUSART1(udata); // otherwise, echo the received character
	                  break;						//
         
         }     										//end switch(udata)
	   
	    }       										//end if(PIR1bits.RC1IF)
    
   
    }          										//end while(1)
}              										//end main

//---------------------------------------------------------------------


void DoCommand(void)									// This routine parses the input buffer
{															// after a <CR> was received.
unsigned int num;


if(isdigit(inpbuf[0]) ||  inpbuf[0] == '-')		// Did we get a numerical input?
   {
   if(command)												// Was numerical input preceded
      {														// by a command to change a
      switch(command)									// parameter?
         {
         case 'P':   kp = atoi(inpbuf);			// proportional gain change
                     break;
         
         case 'I':   ki = atoi(inpbuf);			// integral gain change
                     break;
         
         case 'D':   kd = atoi(inpbuf);			// differential gain change
                     break;
                 
         case 'A':   accel = atoui(inpbuf);		// acceleration change
                     break;
         
         case 'V':   vlim = atoui(inpbuf);		// velocity limit change
                     break;
         
         case 'S':   PR2 = atoub(inpbuf);			// servo update timing change
                     break;
         
         default:    break;
              
         }
      command = 0;
      }
      
   else if(mode == 0) ypwm = atoi(inpbuf);  	// manual mode:  write directly to PWM
      
   else if(mode == 1) velcom = atoi(inpbuf);	// velocity mode:  input data is velocity
                                            
   else if(mode == 2)							// Input data is a relative movement distance
      {												// distance for position mode.
      if(!stat.move_in_progress)				// Make sure no move is in progress.
         {
         phase1dist.i[1] = atoi(inpbuf);  // Load the 16-bit relative movement distance into
         phase1dist.i[0] = 0;             // the upper two bytes of phase1dist variable
         
         fposition.i[0] = position.i[0];	// Final position is commanded position
         fposition.i[1] = position.i[1] 	// + relative move distance
         				+ phase1dist.i[1];
            
         if(phase1dist.b[3] & 0x80)			// If the relative move is negative,   
            {
            stat.neg_move = 1;				// set flag to indicate neg. move
            
            _asm									// and covert phase1dist to a positive
            comf     phase1dist+2,F			// value.
            comf     phase1dist+3,F
            clrf     WREG,F
            incf     phase1dist+2,F
            addwfc   phase1dist+3,F
            _endasm
            
            }
         else stat.neg_move = 0;				// Clear the flag for a positive move.
         
         _asm										// phase1dist now holds the total distance,
            rlcf  phase1dist+3,W          // so divide by 2
            rrcf  phase1dist+3,F
            rrcf  phase1dist+2,F
            rrcf  phase1dist+1,F
            rrcf  phase1dist+0,F
         _endasm          
           
         flatcount.i[1] = 0;				// Clear flatcount
         flatcount.i[0] = 0;
         
         stat.phase = 0;					// Clear flag:  first half of move.
         stat.move_in_progress = 1;					
         }
      }
   else;
   }

else switch(inpbuf[0])
   {
   case 'K':   if(inpbuf[1] == 'P') command = 'P';		// If this is a parameter change,
               else 												// determine which parameter
               if(inpbuf[1] == 'I') command = 'I';
               else 
               if(inpbuf[1] == 'D') command = 'D';
               else 
               if(inpbuf[1] == 'A') command = 'A';
               else 
               if(inpbuf[1] == 'V') command = 'V';
               else
               if(inpbuf[1] == 'S') command = 'S';
               break;
      
   case 'W':   if(PORTFbits.RF4 == 0)
                  {
                  putrsUSART1("\r\nPWM ON"); 
                  SetDCPWM1(512);
                  }
               else
                  {
                  putrsUSART1("\r\nPWM OFF"); 
                  }
               PORTF = PORTF ^ 0x10;      				// enables or disables PWM amplifier
               break;
   
   case 'R':   putrsUSART1("  Kp = ");						// Send all parameters to host.
               uitoa(kp, data);
               putsUSART1(data);
               
               putrsUSART1("  Ki = ");
               uitoa(ki, data);
               putsUSART1(data);
               
               putrsUSART1("  Kd = ");
               uitoa(kd, data);
               putsUSART1(data);
               
               putrsUSART1("  Vlim = ");
               uitoa(vlim, data);
               putsUSART1(data);
               
               putrsUSART1("  Acc. = ");
               uitoa(accel, data);
               putsUSART1(data);
                                          
               break; 
         
   case 'M':   putrsUSART1(" Manual Mode");				// Put the servomotor in manual mode.
               SetDCPWM1(512);
               mode = 0;
               break;
   
   case 'V':   putrsUSART1(" Velocity Mode");			// Put the servomotor in velocity mode.
               velcom = 0;
               SetDCPWM1(512);
               position = mposition;
               fposition = position;
               mode = 1;
               break;
   
   case 'P':   putrsUSART1(" Position Mode");			// Put the servomotor in position mode.
               SetDCPWM1(512);
               position = mposition;
               fposition = position;
               mode = 2;
               break;
        
   case 'L':   tempint0 = mposition.i[0];					// Send measured and commanded position
               tempint2 = position.i[0];					// to host.
               tempint1 = mposition.i[1];
               tempint3 = position.i[1];
               ulitoa(tempint1,tempint0,data);
               putrsUSART1(" Measured = ");
               putsUSART1(data);
               ulitoa(tempint3,tempint2,data);
               putrsUSART1("  Commanded = ");
               putsUSART1(data);
					break;
                 
   case 'Z':   if(!stat.move_in_progress)					// Set measured position to 0.
                  {
                  if(mode) CloseTimer2();					// Disable interrupt generation.
                  position.i[1] = 0;
                  position.i[0] = 0;
                  mposition = position;
                  fposition = position;
                  WriteTimer0(0);
                  WriteTimer3(0);
                  mvelocity.i[1] = 0;
                  mvelocity.i[0] = 0;
                  UpCount = 0;
                  DnCount = 0;
                  if(mode) OpenTimer2(TIMER_INT_ON&T2_SOURCE_EXT);	// Enable Timer2
                  }
               
               putrsUSART1(ready);
               break;
   
   default:    if(inpbuf[0] != '\0')
                  {
                  putrsUSART1(error);
                  }
               break;
      
   }

}

//---------------------------------------------------------------------


void ServoISR(void)
{
PRODHtemp = PRODH;							// Save context for necessary registers
PRODLtemp = PRODL;
FSR0temp = FSR0;
FSR1temp = FSR1;

UpdatePosition();

if(mode)											// This portion of code not executed
	{												// in manual mode.
	UpdateTrajectory();						// Do trajectory algorithm to get new 
													// commanded position.
	aarg = position;							// Subtract measured position
	barg = mposition;							// from commanded position
	sub32();										// to get 32 bit position error.

	poserror.b[2] = aarg.b[3];				// LSByte holds fractional encoder counts,
	poserror.b[1] = aarg.b[2];				// so shift everything right.
	poserror.b[0] = aarg.b[1];

	if (poserror.b[2] & 0x80)				// If position error is negative.
		{
		poserror.b[3] = 0xff;				// Sign-extend to 32 bits.
	
		if((poserror.i[1] != 0xffff) || !(poserror.b[1] & 0x80))
			{
			poserror.i[1] = 0xffff;			// Limit error to 16-bit signed integer.
			poserror.i[0] = 0x8000;
			}
		else;
		}

	else											// If position error is positive.
		{
		poserror.b[3] = 0x00;
	
		if((poserror.i[1] != 0x0000) || (poserror.b[1] & 0x80))
			{
			poserror.i[1] = 0x0000;			// Limit error to 16-bit signed integer.
			poserror.i[0] = 0x7fff;
			}
		else;
		}

	u0 = poserror.i[0];						// Put position error in u0.

	multcnd = u0;								// Calculate proportional term
	multplr = kp;								// of PID
	mult();
	ypid = aarg;

	if(!stat.saturated) integral +=u0;	// Bypass integration if saturated.
	                                    
	multcnd = integral;						// Calculate integral term of PID
	multplr = ki;
	mult();
	barg = ypid;
	add32();										// Add integral term.
	ypid = aarg;
	
	multcnd = u0 - u1;						// Calculate differential term of PID
	multplr = kd;
	mult();
	barg = ypid;								// Add differential term
	add32();
	ypid = aarg;

	if(ypid.b[3] & 0x80)						// If PID result is negative
		{
		if((ypid.b[3] < 0xff) || !(ypid.b[2] & 0x80))
			{
			ypid.i[1] = 0xff80;				// Limit result to 24-bit value
			ypid.i[0] = 0x0000;
			}
		else;
		}

	else											// If PID result is positive
		{
		if(ypid.b[3] || (ypid.b[2] > 0x7f))
			{
			ypid.i[1] = 0x007f;				// Limit result to 24-bit value
			ypid.i[0] = 0xffff;
			}
		else;
		}

	ypid.b[0] = ypid.b[1];					// Shift PID result right to get
	ypid.b[1] = ypid.b[2];					// upper 16 bits of 24-bit result in
	ypwm = ypid.i[0];							// ypid.i[0]
	
	u1 = u0;										// Save current error in u1
	}												// end if(mode)
	
stat.saturated = 0;							// Clear saturation flag

if(ypwm > 500)
   {
   ypwm = 500;
   stat.saturated = 1;
   }
else if(ypwm < -500) 
   {
   ypwm = -500;
   stat.saturated = 1;
   }

SetDCPWM1((unsigned int)(ypwm + 512));	// Write new duty cycle value
	
PRODH = PRODHtemp;							// Restore context.
PRODL = PRODLtemp;
FSR0 = FSR0temp;
FSR1 = FSR1temp;

PIR1bits.TMR2IF = 0;							// Clear flag that generated interrupt.
}

//---------------------------------------------------------------------
// The relative distance travelled during the sample period is found using
// the following formula:
//
// mvelocity = (Timer0 - prev. Timer0) - (Timer3 - prev. Timer3)
// 
// This is done so the timers do not have to be cleared each sample period
// and potentially cause counts to be lost.
//


void UpdatePosition(void)
{
mvelocity.i[0] = DnCount;					// Add previous Timer3 value
mvelocity.i[0] -= UpCount;					// Subtract previous Timer0 value

UpCount = ReadTimer0();						// get new values from Timer0
DnCount = ReadTimer3();						// and Timer3

mvelocity.i[0] += UpCount;					// Add current Timer0 value
mvelocity.i[0] -= DnCount;					// Subtract current Timer3 value

mvelocity.b[2] = mvelocity.b[1];			// Shift result left:  LSbyte is
mvelocity.b[1] = mvelocity.b[0];			// fractional 
mvelocity.b[0] = 0;

if (mvelocity.b[2] & 0x80) 				// Sign-extend result
	mvelocity.b[3] = 0xff;
else 
	mvelocity.b[3] = 0;
  
aarg = mposition;								// Add velocity to measured position
barg = mvelocity;									
add32();
mposition = aarg;

}

//---------------------------------------------------------------------

void UpdateTrajectory(void)
{
if(mode == 1)									// If servomotor is in velocity mode.
   {
   if(!stat.saturated)						// Don't update profile if saturated.
      {
      if(velact.i[1] < velcom)			// If current velocity is less than
         {										// commanded velocity.
         aarg = velact;						
         barg.i[0] = accel;				// Accelerate
         barg.i[1] = 0;
         add32();
         velact = aarg;
         
         if(velact.i[1] > velcom) 		// Don't exceed commanded velocity
         	velact.i[1] = velcom;
         
         if(velact.i[1] > vlim) 			// Don't exceed velocity limit parameter
         	velact.i[1] = vlim;
         }
      else
      if(velact.i[1] > velcom)			// If current velocity exceeds commanded 
         {										// velocity
         aarg = velact;
         barg.i[0] = accel;				// Decelerate
         barg.i[1] = 0;
         sub32();
         velact = aarg;
         if(velact.i[1] < velcom) 		// Don't exceed commanded velocity
         	velact.i[1] = velcom;
         if(velact.i[1] < -vlim) 		// Don't exceed velocity limit parameter
         	velact.i[1] = -vlim;
         }
      else;
                  
      aarg = position;						// Add current commanded velocity to 
      barg.i[0] = velact.i[1];			// the commanded position                       
      if(velact.b[3] & 0x80) 
      	barg.i[1] = 0xffff;
      else barg.i[1] = 0;
      add32();
      position = aarg;
      }
   }

else if(mode == 2)
   {                                   // If we're in position mode.
   if(!stat.saturated)						// Don't update profile if output is saturated
      {
      if(!stat.phase)                  // If we're in the first half of the move.
         {
         if(velact.i[1] < vlim)        // If we're still below the velocity limit for
            {                          // the move:
           	aarg = velact;
         	barg.i[0] = accel;
         	barg.i[1] = 0;
         	add32();
         	velact = aarg;
            }
         else                          // If we're at the velocity limit,
            {                          // increment flatcount to keep track of
            _asm                       // time spent in flat portion of trajectory.
            clrf     WREG,F
            incf     flatcount+0,F
            addwfc   flatcount+1,F
            addwfc   flatcount+2,F
            addwfc   flatcount+3,F
            _endasm
            }
      
         aarg = phase1dist;            // go ahead and subtract the current velocity
         barg.i[1] = 0;                // from the move distance to keep track of the
         barg.i[0] = velact.i[1];      // number of encoder counts travelled during
         sub32();                      // this sample period.
         phase1dist = aarg;
                
         aarg = position;              // Add the current velocity to the commanded position.
         if(stat.neg_move) sub32();
         else add32();
         position = aarg;
         
         if(phase1dist.b[3] & 0x80)		// If phase1dist has gone negative, the first 
         	stat.phase = 1;				// half of the move has completed
                  	
         }
      
      else                              // If we're in the second half of the move.
         {
         if(flatcount.i[1] || flatcount.i[0])
            {
            _asm								// If flatcount is not zero, decrement it.
            clrf     WREG,F
            decf     flatcount+0,F
            subwfb   flatcount+1,F
            subwfb   flatcount+2,F
            subwfb   flatcount+3,F
            _endasm
            }
         else
         	if(velact.i[1]) 				// If velact is not 0, decelerate.
         		{
         		aarg = velact;
         		barg.i[0] = accel;
         		barg.i[1] = 0;
         		sub32();
         		velact = aarg;
         		}
         else									// flatcount is 0, velact is 0, so move is 
         	{									// is over.  Set commanded position equal to 
         	position = fposition;		// the final position calculated at the beginning
         	stat.move_in_progress = 0;	// of the move.
         	}
                                    
         aarg = position;					// Add current velocity to commanded position.
         barg.i[1] = 0;
         barg.i[0] = velact.i[1];
         if(stat.neg_move) sub32();
         else add32();
         position = aarg;
         }
      }                                // END if(!stat.saturated)  
   }                                   // END if(mode == 2)

else;

}

//---------------------------------------------------------------------

void add32(void)
{
_asm

   MOVFP    barg+0,WREG
   ADDWF    aarg+0,F
   MOVFP    barg+1,WREG
   ADDWFC   aarg+1,F
   MOVFP    barg+2,WREG
   ADDWFC   aarg+2,F
   MOVFP    barg+3,WREG
   ADDWFC   aarg+3,F

_endasm
}

//---------------------------------------------------------------------

void sub32(void)
{
_asm

   MOVFP    barg+0,WREG
   SUBWF    aarg+0,F
   MOVFP    barg+1,WREG
   SUBWFB   aarg+1,F
   MOVFP    barg+2,WREG
   SUBWFB   aarg+2,F
   MOVFP    barg+3,WREG
   SUBWFB   aarg+3,F

_endasm
}

//---------------------------------------------------------------------

void mult(void)
{
_asm
      
   movfp    multcnd+0,WREG
   mulwf    multplr+0
   movpf    PRODH,aarg+1
   movpf    PRODL,aarg+0
   
   movfp    multcnd+1,WREG
   mulwf    multplr+1
   movpf    PRODH,aarg+3
   movpf    PRODL,aarg+2
   
   movfp    multcnd+0,WREG
   mulwf    multplr+1
   
   movfp    PRODL,WREG
   addwf    aarg+1,F
   movfp    PRODH,WREG
   addwfc   aarg+2,F
   clrf     WREG,F
   addwfc   aarg+3,F
   
   movfp    multcnd+1,WREG
   mulwf    multplr+0
   
   movfp    PRODL,WREG
   addwf    aarg+1,F
   movfp    PRODH,WREG
   addwfc   aarg+2,F
   clrf     WREG,F
   addwfc   aarg+3,F
   
   btfss    multplr+1,7
   goto     $ + 5
   movfp    multcnd+0,WREG
   subwf    aarg+2,F
   movfp    multcnd+1,WREG
   subwfb   aarg+3,F
   
   btfss    multcnd+1,7
   goto     $ + 5
   movfp    multplr+0,WREG
   subwf    aarg+2,F
   movfp    multplr+1,WREG
   subwfb   aarg+3,F
   
   nop   
_endasm
}

//---------------------------------------------------------------------

void ulitoa(unsigned int value1, unsigned int value0, char *string)
{
unsigned int temp;

temp = value1;
*string = ntoh(temp >> 12);
string++;

temp = value1 & 0x0f00;
*string = ntoh(temp >> 8);
string++;

temp = value1 & 0x00f0;
*string = ntoh(temp >> 4);
string++;

temp = value1 & 0x000f;
*string = ntoh(temp);
string++;

temp = value0;
*string = ntoh(temp >> 12);
string++;

temp = value0 & 0x0f00;
*string = ntoh(temp >> 8);
string++;

temp = value0 & 0x00f0;
*string = ntoh(temp >> 4);
string++;

temp = value0 & 0x000f;
*string = ntoh(temp);
string++;

*string = 0;

return;
}

//---------------------------------------------------------------------

char ntoh(unsigned int value)
{
char hexval;

if(value < 10) hexval = value + '0';
else if(value < 16) hexval = value - 10 + 'A';

return hexval;
}

//---------------------------------------------------------------------

void InitVars(void)
{
i = 0;

kp = 2000;
ki = 15;
kd = 6000;

vlim = 4096;
velcom = 0;
velact.i[1] = 0;
velact.i[0] = 0;
accel = 65535;

integral = 0;
mvelocity.i[1] = 0;
mvelocity.i[0] = 0;
UpCount = 0;
DnCount = 0;
position = mposition;
fposition = position;

stat.move_in_progress = 0;
stat.neg_move = 0;
stat.phase = 1;

mode = 0;
ypwm = 0;

strset(inpbuf,'\0');
}

//---------------------------------------------------------------------

void InitPorts(void)
{
ADCON1 = 0x0E;          //ensure port F is configured for digital IO
PORTF = 0x00;           //ensure port F is 0 before setting data direction
DDRF = 0x0f;            //RF<7:4> outputs, RF<3:0> inputs

PORTFbits.RF4 = 0;      //ensure pwm amplifier is disabled!!!

// Up/Down Register Setup -----------------------

WriteTimer0(0);								
WriteTimer3(0);
OpenTimer0(TIMER_INT_OFF&T0_EDGE_FALL&T0_SOURCE_EXT&T0_PS_1_1);
OpenTimer3(TIMER_INT_OFF&T3_SOURCE_EXT);
TCON2bits.CA1 = 1;

// PWM Setup ------------------------------------

OpenTimer1(TIMER_INT_OFF&T1_SOURCE_INT&T1_T2_8BIT);     	// set up timer1 for PWM timebase    
OpenPWM1(0xff);               									//start up PWM1
SetDCPWM1(512);               									//set the initial PWM duty cycle to ~50%


PR2 = 0x08;																// Set Timer2 overflow period to 8
																			// for 3.9 KHz update at 33 MHz
OpenTimer2(TIMER_INT_ON&T2_SOURCE_EXT);						// Enable Timer2

// USART1 Setup ---------------------------------

OpenUSART1(USART_TX_INT_OFF&USART_RX_INT_OFF&USART_ASYNCH_MODE&
		USART_EIGHT_BIT&USART_CONT_RX, 26);                // open the serial port
																			// 19.2 kbaud @ 33 Mhz
}


