/***********************************************************************/
/*                                                                     */
/*  FILE        :HelpRobot04.c                                         */
/*  DATE        :Sat, Aug 02, 2008                                     */
/*  DESCRIPTION :main program file.                                    */
/*  CPU GROUP   :26                                                    */
/*                                                                     */
/*  This file is generated by Renesas Project Generator (Ver.4.8).     */
/*                                                                     */
/***********************************************************************/

//
// ROBO-ONE お手伝いロボットプロジェクト用プログラム
//
//　＜用意するもの＞
//　・KRC-3
//　・SERIAL USB ADAPTER
//　・３線ケーブル２本（赤い線をコネクターから外しておく）
//　
// 　＜初期設定＞
//　・KRC-3はアナログを受信するために７バイトモードに設定しておくこと
//　・３線ケーブルでNetTansorとKCB-1のCOMポートを接続する
//　・３線ケーブルでKCB-1のSIO2ポートとRCB-3のRXポートを接続する
//　・HTHのConfigで「KRC-1を使う」からチェックを外す（拡張７バイトモードを使用する）
//　・HTHでKRC-3でロボットが動かせるようにボタンとモーションの設定を済ませておく
//
// Copyright (C) 2008 KONDO KAGAKU CO.,LTD. All Rights Reserved.

#include <com.h>
#include <sio.h>
#include <led.h>
#include <krc.h>
#include <pio.h>

#define KRC3_CMD      0x80
#define KRC3_CMD_CNT  8

_Bool check_sum (BYTE *dat)
{
	BYTE i, sum = 0;
	
	for (i = 0; i < KRC3_CMD_CNT - 2; i++) // dat[0] ~ dat[6]（７個）の和を求める
	{
		sum += *(dat + i);
	}
	
	if ((sum & 0x7F) == *(dat + KRC3_CMD_CNT - 1)) // 受信したチェックサムと比較
	{
		return TRUE;  // データは正しいコマンドでした
	}
	else 
	{
		return FALSE; // 受信を失敗しているか、間違えたデータでした
	}		
}

void sio2_tx ( // 返値）なし
	BYTE *data,   // 送信データ 
	size_t size   // 送信データのサイズ[Byte]
)
{
	BYTE i;

	for (i = 0; i < size; i++) {
		uart2_putchar (*(data + i)); // 1づつByte送信する
	}	
}

void main(void)
{
	char c; // １文字受信用変数
	BYTE i; // カウンター用変数

	BYTE krc3buf[8];     // 受信用データ
	BYTE d_cnt = 0;      // データ受信バッファカウンター
	unsigned int adv1;   // KRC-3のスティックアナログデータ保存用
	unsigned int adv2;   // AD1, AD2
	unsigned int button; // ボタンデータ保存用変数     
	
	// CPUを初期化します
	cpu_init ();
	
	// 割り込み(今回はCOMの受信割り込み)を許可します
	//cpu_int_set ();	
	
	// 電源確認用LED点灯
	ledgrn_on ();
	
	// COMポートでNetTansorと通信する設定を行います
	com_open (BR9600, 8, 1, PARITY_NONE);
	//com_open (BR2400, 8, 1, PARITY_NONE, TRUE); // KRC-3とCOMポートを直接つないでテストする場合
	
	// SIO1ではKRS-40xxを動かします
	sio1_init ();
	
	// SIO2ではRCB-3にコマンドを送信します
	// 低速シリアル通信モードを使用しています
	uart2_asyncmode (BR2400, 8, 1, PARITY_NONE, TRUE);

	iopol_u2mr = 1;      // 送信データの極性を反転します
	uart2_send_start (); // 送信開始(送信のみを許可します)
	
	// データ受信バッファにヘッダコマンドだけ入力しておきます
	krc3buf[0] = KRC3_CMD;	
	
	while (1) {
		//
		// まずポーリングで受信したデータをコマンドとして完成させます
		//
		c = com_getchar ();
		
		if (c == KRC3_CMD) // KRC3からヘッダーを受け取ったとき
		{
			ledred_switch();			 

			for (i = 1; i < KRC3_CMD_CNT; i++) { // 残りの７バイトを全て受信します
				krc3buf[i] = com_getchar ();
			}

			if (check_sum (krc3buf) == TRUE) {  // 正しいチェックサムを受け取っていた場合は
				//
				// SIO2(RCB-3)へデータ(７バイトモードコントロール入力値)を送信します
				//
				uart2_tx (krc3buf, KRC3_CMD_CNT);
			
				//
				// ここからは受信コマンドを解析し、SIO1に接続されたサーボを動かします
				//
				// ＜例＞
				//　・手元のKRC-3はスティック範囲がAD1=26~108, AD2=19~107だった
				//  ・ADの値を128倍にすると、KRSの可動範囲とほぼ一致する
				//
				adv1 = krc3buf[3]; // いったんunsigned intに代入しないと、桁あふれを起こす
				adv1 <<= 7;
				adv2 = krc3buf[4];
				adv2 <<= 7;
				
				//
				// シリアルサーボモーターを動かす
				//
				sio1_set_pos (0, adv1);
				wait(300);
				sio1_set_pos (1, adv2);
				wait(300);	
				
				//
				// その他ボタンに割り当てたモーションを再生します
				//
				// まずボタンデータを作成します
				button = krc3buf[1];
				button = (button << 8) + krc3buf[2];

				// ボタンのコントロール入力値に対応したモーションを再生(シリアルサーボ)
				/*
				switch (button) {
					case BTN7B_RR:
						sio1_set_pos ();
						break;
					default
						break;
				}
				*/
			}
		}
	}		
}