/*******************************************************
*  ラジコンロボット送信機プログラム
* 　ジョイスティックのデータをA/Dで入力し、USARTで送信し無線出力
******************************************************/
#include <16F876A.h>
#fuses HS,NOWDT,NOPROTECT,PUT,BROWNOUT,NODEBUG,NOLVP 
#device ADC=10						// A/D変換10ビットモード
/// USART初期設定
#use delay(CLOCK = 10000000)
#use rs232(BAUD = 2400, XMIT = PIN_C6, RCV = PIN_C7, ERRORS)
//// 変数定義
char	Dir, SendBuf[6];
unsigned long	HData, VData;
unsigned long	Duty, Duty1, Duty2;
/// プロトタイピング		
void CalcPWM(void);
void Send(void);

//// メイン関数
void main(void)
{
	//// ポート初期設定
	set_tris_c(0x80);				// ポートCはUSART受信以外全て出力
	set_tris_b(0xF0);				// ポートB上位入力下位出力(未使用）
	set_tris_a(0x0B);				// ポートAN0,AN1,AN3アナログ入力
	port_b_pullups(TRUE);			// ポートBのプルアップオン
	output_low(PIN_C0);				// LED0点灯
	/// ADコンバータの初期設定
	setup_adc_ports(AN0_AN1_AN3);	// RA0,RA2,RA3ピンがアナログ入力
	setup_adc(ADC_CLOCK_DIV_32);	// Fosc/32 速度設定
	/// メインループ
	while(1)
	{
		/// LED	制御
		output_toggle(PIN_C1);
		output_toggle(PIN_A2);
		/// ジョイスティックのデータ入力
		set_adc_channel(0);			// チャネル０選択
		delay_us(20);				// アクイジション待ち
		VData = read_adc();			// A/D変換データ10ビット読み込み
		set_adc_channel(1);			// チャネル1選択
		delay_us(20);				// アクイジション待ち
		HData = read_adc();			// A/D変換データ10ビット読み込み	
		/// モータPWM値計算
		CalcPWM();					// PWMを求める
		/// 送信
		Send();						// 無線送信実行
	}
}
		
/************************************
* PWM値計算サブ関数
*************************************/
void CalcPWM(void)
{
	VData = VData >> 2;				// 8ビットに補正
	HData = HData >> 2;
	/// 速度設定	
	/// 00-7F バック　7Fのとき速度0、 80-FF 前進　　80のとき速度0	
	if(VData >= 0x80) {				// 0x80以上か？
		/// 前進
		Dir = '0';					// 方向前進設定
		Duty = (unsigned long)((VData - 0x80) * 2);
	}
	else {							// 0x7F以下の場合
		/// 後進
		Dir = '1';					// 方向後進設定
		Duty = (unsigned long)((0x7F - VData) * 2);
	}
	//// 旋回制御
	if(HData >= 0x80) {				// 0x80以上か？
		/// 左旋回　Duty1を増加　Duty2を減少
		HData = Hdata - 0x80;		// 差分
		if(Duty < HData)			// Duty2設定
			Duty2 = 0;				// Duty-HData　負なら0に制限
		else
			Duty2 = Duty - HData;
		Duty1 = Duty + HData;		// Duty1設定
		if(Duty1 > 0xFF)
			Duty1 = 0xFF;			// 0xFF以下に制限
	}
	else {
		/// 右旋回　Duty1を減少　Duty2を増加
		HData = 0x7F - HData;		// 差分
		if(Duty < HData)			// Duty1設定
			Duty1 = 0;				// Duty-HDAta　負なら0に制限
		else
			Duty1 = Duty - HData;
		Duty2 = Duty + HData;		// Duty2設定
		if(Duty2 > 0xFF)
			Duty2 = 0xFF;			// 0xFF以下に制限
	}
}

/********************************
* データ送信サブ関数
********************************/
void Send(void)
{
	int i;
	/// 送信データ作成
	SendBuf[0] = Dir;				// モータ１　方向データ
	SendBuf[1] = (Duty1/0x10)+0x30;	// PWM上位4ビット
	SendBuf[2] = (Duty1%0x10)+0x30;	// PWM下位4ビット
	SendBuf[3] = Dir;				// モータ２　方向データ
	SendBuf[4] = (Duty2/0x10)+0x30;	// PWM上位4ビット
	SendBuf[5] = (Duty2%0x10)+0x30;	// PWM下位4ビット
	/// データ送信開始
	putc('S');						// 文字S送信
	putc('S');
	for(i=0; i<6; i++) {		
		putc(SendBuf[i]);			// モータデータ送信
	}
	for(i=0; i<6; i++) {
		putc(~SendBuf[i]);			// 反転データ送信
	}
	putc('E');						// 文字E送信
}