/*******************************************************
*  ラジコンロボット受信ユニットプログラム
* 　無線のデータをUSARTで取り込み、２個のDCモータを２つのPWMで制御
*******************************************************/
#include <16F876A.h>
#fuses HS,NOWDT,NOPROTECT,PUT,BROWNOUT,NODEBUG,NOLVP 
/// USART初期設定
#use delay(CLOCK = 10000000)
#use rs232(BAUD = 2400, XMIT = PIN_C6, RCV = PIN_C7, ERRORS)
//// 変数定義
#byte	PORTA = 5
#byte	PORTB = 6
#byte	PORTC = 7
char	RcvData, RcvBuf1[10], RcvBuf2[10];
unsigned int 	RcvState, RcvCount, ErrFlag;
unsigned long	Duty1, Duty2;
/// プロトタイピング		
void MotorCnt(void);

//// メイン関数
void main(void)
{
	int i;
	//// ポート初期設定
	setup_adc_ports(NO_ANALOGS);			// 全ピンディジタルモード	
	set_tris_c(0x80);						// ポートCはUSART受信以外全て出力
	set_tris_b(0xF0);						// ポートB上位入力下位出力
	set_tris_a(0);							// ポートA全て出力（未使用）
	port_b_pullups(TRUE);					// ポートBのプルアップオン
	portb = 0x0f;							// 全LED消去
	/// CCPの初期設定
	setup_ccp1(CCP_PWM);					// CCP1,CCP2をPWMモードに
	setup_ccp2(CCP_PWM);
	/// タイマ２の設定					
	setup_timer_2(T2_DIV_BY_1,0xFF,1);		// 10ビット分解能
	set_pwm1_duty(0);						// 停止		
	set_pwm2_duty(0);
	/// ブリッジ設定　停止
	portc = 0;								// 全LED消灯
	output_low(PIN_B3);						// スタートLED点灯
	RcvState = 0;							// ステート初期化
	RcvCount = 0;							// 受信カウンタ初期化
	///// メインループ
	while(1)
	{
		RcvData = getc();						// データ受信
		switch(RcvState)						// ステートで分岐
		{
			case 0:	if(RcvData == 'S')			// 初期ステート S受信で＋１
						RcvState = 1;
					break;
			case 1:	if(RcvData == 'S')			// 次のS受信で＋１
						RcvState = 2;
					else
						RcvState = 0;			// 異なれば初期ステートへ戻る
					break;
			case 2:	RcvBuf1[RcvCount] = RcvData;// データ受信ステート
					RcvCount++;					// 受信カウンタ更新
					if(RcvCount >= 6){
						RcvCount = 0;			// 受信カウンタクリア
						RcvState = 3;			// 6バイト受信でステート＋１
					}
					break;
			case 3:	RCVBuf2[RcvCount] = ~RcvData;// 反転データ受信　反転して格納	
					RcvCount++;					// 受信カウンタ更新
					if(RcvCount >= 6){
						RcvCount = 0;			// 受信カウンタクリア
						RcvState = 4;			// ステート＋１
					}
					break;
			case 4:	if(RcvData == 'E')			// 終了E受信か
					{
						ErrFlag = 0;			// 照合チェック
						for(i=0; i<6; i++) {
							if(RcvBuf1[i] != RcvBuf2[i])
								ErrFlag = 1;
						}
						if(ErrFlag == 0)		// 照合OKか？
						{
							// 照合OK　モータ制御
							output_toggle(PIN_B2);// 緑LED点滅
							output_high(PIN_B0);
							MotorCnt();			// モータ制御実行
						}
						else
							output_low(PIN_B0);	// エラー時赤色LED点灯
					}
					RcvState = 0;				// ステート初期状態へ戻る
					break;
			default:RcvState = 0;
					break;
		}
	}
}

/**************************************
* モータ制御サブ関数
***************************************/
void MotorCnt(void)
{
	/// モータ１の制御
	if(RcvBuf1[0] == '0'){
		output_low(PIN_C3);				// 正転の制御
		output_high(PIN_C0);
	}
	else {
		output_high(PIN_C3);			// 逆転の制御
		output_low(PIN_C0);
	}
	/// モータ1速度設定　8ビットを10ビットに変換
	Duty1 = ((RcvBuf1[1] & 0x0F) << 4) + (RcvBuf1[2] & 0x0F);
	set_pwm1_duty(Duty1 << 2);		
	/// モータ２の制御
	if(RcvBuf1[3] == '0'){
		output_low(PIN_C5);				// 正転制御
		output_high(PIN_C4);
	}
	else {
		output_high(PIN_C5);			// 逆転制御
		output_low(PIN_C4);
	}
	/// モータ2速度設定
	Duty2 = ((RcvBuf1[4] & 0x0F) << 4) + (RcvBuf1[5] & 0x0F);
	set_pwm2_duty(Duty2 << 2);
}