#include <io18f4520.h>

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

void servos(void);

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

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
  INTCON2 = 0x04;
  ADCON1 = 0x0F;
  INTCON = 0x80;
  T0CON = 0x88;
  TMR0L = 0xBF;
  TMR0H = 0x63;
  PORTC = 0x00;
  PORTD = 0xf0;

  // to use portA as digital inputs, must switch on ADC and set all pins to digital
//  ADON = 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);
    }

  }
}

#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;
  RA0=1;
  for(i=0; i<6000; i++);
  for(i=0; i<servo1; i++);
  RA0=0;
  RA1=1;
  for(i=0; i<6000; i++);
  for(i=0; i<servo2; i++);
  RA1=0;
}