//*************** サーボ１６制御プログラム **********************//
//	シリアルデータを読み取って　サーボをコントロール
//　データのフォーマットは
//  サーボ番号：０～Ｆ　の１６個
//  回転角度　：±４０００を大よその限界値とする
//  注意：規定値以上のデータを入力した場合、タイマーの値が異常値になりサーボ用パルスは出せない
//　
//  
//　サーボ番号に続いて回転角度のデータが送られてくる
//  サーボ番号1byte '－'1byte 角度5桁　5byte 　最長で7byteのデータが送られてくる
//
//
// ver 1.0 
//		基本プログラム作成													2009.12.15
// ver 1.1
// 		命令受信時にＣＣＳのバグと見られる問題を発見　回避用の命令を追加	2009.12.15
//		命令の角度値を５バイトに固定										2009.12.15
//***************************************************************//

#include<16F877A.h>
#fuses HS,NOWDT,NOPROTECT,PUT,BROWNOUT,NOLVP
#use delay(CLOCK = 20000000)
#use rs232(BAUD = 9600,XMIT = PIN_C6, RCV = PIN_C7)
#define ZERO_P 53036
#define CENTER 60536
unsigned int select=0;
unsigned long data1[8] = {CENTER,CENTER,CENTER,CENTER,CENTER,CENTER,CENTER,CENTER};
unsigned long data2[8] = {CENTER,CENTER,CENTER,CENTER,CENTER,CENTER,CENTER,CENTER};
#int_timer1
void isr1(){
	
	set_timer1(0xCFC2);
	CCP_1 = data1[select];
	CCP_2 = data2[select];
	switch(select){
		case 0: output_b(0x01); output_a(0x01);output_e(0x00);break;
		case 1: output_b(0x02); output_a(0x02);output_e(0x00);break;
		case 2: output_b(0x04); output_a(0x04);output_e(0x00);break;
		case 3: output_b(0x08); output_a(0x08);output_e(0x00);break;
		case 4: output_b(0x10); output_a(0x10);output_e(0x00);break;
		case 5: output_b(0x20); output_a(0x20);output_e(0x00);break;
		case 6: output_b(0x40); output_a(0x00);output_e(0x01);break;
		case 7: output_b(0x80); output_a(0x00);output_e(0x02);break;
		default : output_b(0x01);output_a(0x01);output_e(0x00);
	}
	select++;
	
	if(select >=8) select = 0;
}

#int_ccp1
void ccp1_isr(){
	output_b(0x00);
	
}

#int_ccp2
void ccp2_isr(){
	output_a(0x00);
	output_e(0x00);
}

#int_rda
void isr_rcv(){
	static char buffer[10];
	static int t = 0;
	signed long rad;
	signed long tmp;
	char moji;
	
	moji = getc();
	if(moji == 0x0d || moji == 0x0a){
		if(buffer[1] == '-'){
			tmp = (buffer[4]-'0')*1000;											//ＣＣＳコンパイラのバグ回避（ＰＣＭ　ver 3.222)
			tmp = (long)(tmp / 10);
			rad = ((buffer[2]-'0')*10000 + (buffer[3]-'0')*1000) + (buffer[5]-'0')*10 + (buffer[6]-'0');
			rad = -(rad + tmp);
		}
		else{
			tmp = (buffer[3]-'0')*1000;
			tmp = (long)(tmp / 10);
			rad =   (buffer[1]-'0')*10000 + (buffer[2]-'0')*1000 + (buffer[4]-'0')*10 + (buffer[5]-'0');
			rad = rad + tmp;
			
		}
		switch(buffer[0]){
			case '0': data1[0] = CENTER + rad;break;
			case '1': data1[1] = CENTER + rad;break;
			case '2': data1[2] = CENTER + rad;break;
			case '3': data1[3] = CENTER + rad;break;
			case '4': data1[4] = CENTER + rad;break;
			case '5': data1[5] = CENTER + rad;break;
			case '6': data1[6] = CENTER + rad;break;
			case '7': data1[7] = CENTER + rad;break;
			case '8': data2[0] = CENTER + rad;break;
			case '9': data2[1] = CENTER + rad;break;
			case 'A': data2[2] = CENTER + rad;break;
			case 'B': data2[3] = CENTER + rad;break;
			case 'C': data2[4] = CENTER + rad;break;
			case 'D': data2[5] = CENTER + rad;break;
			case 'E': data2[6] = CENTER + rad;break;
			case 'F': data2[7] = CENTER + rad;break;

		}
		//printf("%ld %lu\n\r",rad,data1[0]);						//デバッグ用
		t=0;
		
	}
	else{
		buffer[t] = moji;
		t++;
		if(t>8) t=0;
	}
}


void main(){
	set_tris_a(0);
	set_tris_b(0);
	set_tris_e(0);
	setup_timer_1(T1_INTERNAL | T1_DIV_BY_1);
	set_timer1(0xCF2C);

	setup_ccp1(CCP_COMPARE_INT);
	setup_ccp2(CCP_COMPARE_INT);
	
	enable_interrupts(INT_CCP1);
	enable_interrupts(INT_CCP2);
	enable_interrupts(INT_RDA);
	enable_interrupts(INT_TIMER1);
	enable_interrupts(GLOBAL);
	output_a(0x00);
	output_b(0x00);
	output_e(0x00);
	while(1){
		
	}
}