#include<htc.h>

__CONFIG(FOSC_INTOSC & WDTE_OFF & PWRTE_OFF
 & MCLRE_OFF & CP_OFF & CPD_OFF & BOREN_ON
 & CLKOUTEN_OFF & IESO_OFF& FCMEN_OFF);
__CONFIG(PLLEN_ON & LVP_OFF);

#define SVO PORTAbits.RA0
#define SV1 PORTAbits.RA1
#define SV2 PORTAbits.RA2
#define SV3 PORTAbits.RA3
#define SV4 PORTAbits.RA4
#define SV5 PORTAbits.RA5
#define SV6 PORTAbits.RA6
#define SV7 PORTAbits.RA7


volatile unsigned char svoPos[8];	//servo position

int svoNum;

void wait_ms(int value){
	T2CON = 0b00000111;		//T2=ON
	int i;
	for(i = 0; i < value; i++){
		TMR2 = 0;
		while(1){
			if(TMR2 > 64) break;	
		}
	}
	T2CON = 0b00111001;		//T2=OFF
}

static void interrupt 
isr(void){
	if(TMR1IF == 1){
		TMR1IF = 0;			//intrrupt crear
		TMR1H = 250;		//counter reset(2.5mSec long)
		TMR1L = 35;
		TMR4 = 0;			//puls width counter crear
		PR4 = svoPos[svoNum];		//puls width set
		TMR4ON = 1;
		//puls ON
		switch(svoNum){
			case 0:
				SVO = 1;
				break;
			case 1:
				SV1 = 1;
				break;
			case 2:
				SV2 = 1;
				break;
			case 3:
				SV3 = 1;
				break;
			case 4:
				SV4 = 1;
				break;
			case 5:
				SV5 = 1;
				break;
			case 6:
				SV6 = 1;
				break;
			case 7:
				SV7 = 1;
				break;
		}
	}else if(TMR4IF == 1){
		TMR4IF = 0;			//interrupt crear
		TMR4ON = 0;
		PORTA = 0b00000000;	//puls OFF
		svoNum++;					//next phase
		if(svoNum > 7) svoNum = 0;	//1 turn?
	}
}

void main(){
	int i;
	
	//---Clock setting---
	OSCCON = 0b01111000;	//16MHz
	
	//---IO setting---
	ANSELA = 0b00000000;	//All PORTA are Digital
	TRISA = 0b00000000;		//0:output 1:input
	ANSELB = 0b11111110;	//RB0 is Digital
	TRISB = 0b11111110;		//0:output 1:input
	
	//---Timer1 setting---
	T1CON = 0b00110000;		//T1 setting (FOSC/4, 1:8 = 1000count/mS) 
	TMR1IF = 0;
	TMR1IE = 1;				//T1 interrupt enable
	TMR1H = 250;			//counter reset(2.5mSec long)
	TMR1L = 35;
	
	//---Timer4 setting---
	T4CON = 0b00000011;		//FOSC/4, 1:64 62.5count/mS @16MHz
	TMR4IF = 0;
	TMR4IE = 1;				//T4 interrupt disable
	TMR4 = 0;

	//---Interrupt set---
	PEIE = 1;		//Periferal interrupt enable
	GIE = 1;		//General interrupt enable
	
	//---Timer start---
	TMR1ON = 1;
	TMR4ON = 0;
	
	for(i=0; i<8; i++){
		svoPos[i] = 94;
	}
	svoNum = 0;
	
	//---servo drive---
	while(1){
		svoPos[1] = 94;
		svoPos[2] = 94;
		svoPos[3] = 94;
		wait_ms(2000);
		svoPos[1] = 50;
		svoPos[2] = 120;
		svoPos[3] = 35;
		wait_ms(2000);
	}
}