#include <p18f4520.h>	// For MPLAB C18 LITE

#pragma config WDT = OFF
#pragma config OSC = HS


int servo1 = 0x0f;
int servo2 = 0xf0;

void servos(void);

void DelayUs(int count)   //Gives a delay of 40 Us
{
  int i;
  for(i=0;i<count;i++);
  {
  }
}

void DelayMs(int count)   //Gives a delay of 1 Ms
{
  int i;
  int j;
  for(i=0;i<count;i++)
  {
    for(j=0;j<25;j++);
  }
}

/* This is the main program that changes the output LED display. */
void main(void)
{
  int i;
  TRISA = 0x00; //port A as outputs --- servos
  TRISB = 0xff; //Port B as inputs for push buttons.
  TRISC = 0x00; //port C as outputs
  TRISD = 0x00; //port D as outputs --- inbuilt LEDs

 /* //initialize all outputs, if you know how to implement interrupts
  INTCON2 = 0x04; 	//Interrupt register for PortB
  ADCON1 = 0x0F;	//A/D control register
  INTCON = 0x80; 	//Interrupt control register with flag, enable, priority bits
  T0CON = 0x88;		//Timer control register
  TMR0L = 0xBF;		//Timer register, low bytes
  TMR0H = 0x63;		//Timer register, high bytes
  PORTC = 0x00;		//I/Os
  PORTD = 0xf0;		//LEDs
*/

/*  to use portA as digital inputs, must switch on ADC and set all pins to digital
  ADCON = 1;
  ADCON1 = 0x0f;

  DelayMs(500); //delay half sec
*/
  while(1)
  {
    //reset the LEDs
    for(i=0; i<255; i++)
    {
     PORTD = 0x0f;
     DelayMs(200);
	 servos();
     PORTD = 0xf0;
     DelayMs(200);

    }

  }
}

//	Interrupt routine, if you know how to implement interrupts
/* #pragma vector=0x08
__interrupt void my_servo(void);
{
//  int i;
//  RA0=1;
//  for(i=0; i<6000; i++);
//  for(i=0; i<servo1; i++);
//  RA0=0;
  TMR0L = 0xBF;
  TMR0H = 0x63;
  servo1 -= 1;
  if(servo1==0) servo1=0xff;
  INTCON = 0x80;
}
*/

// Sample routine to control 2 Servo motors. Can add up to 6 accordingly.
void servos()
{
  int i;
  PORTAbits.RA0=1;
  for(i=0; i<600; i++);
  for(i=0; i<servo1; i++);
  PORTAbits.RA0=0;
  PORTAbits.RA1=1;
  for(i=0; i<600; i++);
  for(i=0; i<servo2; i++);
  PORTAbits.RA1=0;
}