/***********************************************************************/
/*                                                                     */
/*  FILE        :KHR_1_PSDprogram.c                                    */
/*  DATE        :Thu, Jul 03, 2008                                     */
/*  DESCRIPTION :main program file.                                    */
/*  CPU GROUP   :26                                                    */
/*                                                                     */
/*  This file is generated by Renesas Project Generator (Ver.4.8).     */
/*                                                                     */
/***********************************************************************/
//
// rcb3_put_2Bcode関数を使って、RCB-1のモーションを再生します
//
// プログラム実行条件
// 
// (1) HTHでボードを送信機で操作(2BYTEcode)できるように設定してください
//	   また、前進やサイドステップなどのモーションの割付を行ってください
//
// (2) ADポートへのセンサーの取り付け
//     AD1) RCB(1または3,3J)の空いているポートに信号線(白)とグランド(黒)のみを接続
//     AD2) 起動用スイッチ
//	   AD3) PSDセンサー（前頭部）
//     AD4) PSDセンサー（右肩）
//     AD5) PSDセンサー（左肩）
//     AD6) PSDセンサー（後頭部）
//
// (3) プログラムの流れ
//	   1. 電源投入後、KCB-1ボード上の赤LEDが点灯するまで待つ
//	   2. AD2に接続されたスイッチが押されるまでプログラムは待機
//     3. スイッチを押すとお辞儀モーションをしてプログラムスタート
//	   4. 各部センサーの値が最も大きいところ（一番近いところ）に割り付けたモーション再生
//
// Copyright (C) 2007,2008 KONDO KAGAKU CO.,LTD. All Rights Reserved.
//


#include <ad.h>
#include <rcb3.h>
#include <led.h>

int adj[4], adk[4];	//adj[]はセンサーから読み取った値を代入
					//adk[]は読み取った場所の番号を代入

// プログラム中で実行するモーションをHTHで割付してください
#define FORWARD		BTN2B_LU 			// 前進					左側上ボタン
#define BACKWARD	BTN2B_LD			// 後進					左側下ボタン
#define RIGHTSTEP   BTN2B_LR 			// 右サイドステップ		左側右ボタン
#define LEFTSTEP    BTN2B_LL 			// 左サイドステップ		左側左ボタン
#define RIGHTTURN   BTN2B_LR | BTN2B_LD // 右ターン				左側右ボタン + 下ボタン
#define LEFTTURN    BTN2B_LL | BTN2B_LD // 左ターン				左側左ボタン + 下ボタン
#define BOW			BTN2B_RU			// お辞儀				右側上ボタン

#define NP_WA		1200000	//モーション終了後の待機時間
							//(前に再生したモーションとかぶらないために余裕を持って設定する)

//---------------------------------------
//	バブルソート
//---------------------------------------
void bubble_sort (	//アナログの最大値を求める
	BYTE s, // 検索開始位置
	BYTE t	// 検索終了位置
)
{
	BYTE i, j, k;
	unsigned int tmp;
	
	for(k = s; k <= t; k++){	//センサーから読み取った場所の番号を代入
		adk[k] = k;				//搭載センサーは4つなので0〜3を代入する
	}

	for(i = s; i <= t-1; i++){
		for (j = i+1; j <= t; j++){
			if (adj[j] > adj[i]){	//二つの値を比べる
				tmp = adj[j];		//一度別の変数に代入する
				adj[j] = adj[i];	//adj[i]の値をadj[i+1]へ移動
				adj[i] = tmp;		//別の変通に代入していた値をadj[i]に代入
				
				tmp = adk[j];		//adj1[t] > adu1[s]だったらken2も同じ処理を行う。
				adk[j] = adk[i];	//最大値のモーション番号はken2[0]に代入される。
				adk[i] = tmp;
			}
		}
	}
}


//---------------------------------------
//	モーション再生
//---------------------------------------
void khr_2Bmotion(int cc)			//2BYTEcodeモーション再生関数
{
	rcb3_put_2Bcode (cc);			//2BYTEcodeでモーション再生
	while (ad_read(1) > 500) {}		//モーションが終了するまで(ポートがHからLになるまで)待つ
	rcb3_put_2Bcode (BTN2B_NP);		//モーション終了後ニュートラルを送信
	wait (NP_WA);					//少しだけ待つ
}


//---------------------------------------
//	メイン
//---------------------------------------
void main(void)
{
	int i;
	
	cpu_init (); 					// CPUの初期設定を行う
	ad_oneshotmode (); 				// ADはワンショットモードで読み込む
	ledgrn_on ();      				// 起動確認用LEDを点灯する
	
	rcb3_rx_open ();				// RCBのRXポートを接続します
	wait (5000000); 		 		// RCBの起動が安定するまで待ちます
	ledred_on();					// 赤LED点灯後、モーション再生可能


	while (ad_read(2) < 500) {		//AD2に接続されたボタンが押されるまでここで待機
		rcb3_put_2Bcode (BTN2B_NP);	//待機中はニュートラルを出し続ける
	}

	khr_2Bmotion(BOW);				//お辞儀モーション再生


	while(1){
		rcb3_put_2Bcode (BTN2B_NP);	//ループするごとに必ずニュートラルを送信する		
		
		for (i = 0; i < 4; i++){	//各部に取り付けた4つのセンサーの値を
			adj[i] = ad_read(i+3);	//順番に読み取る
		}
		
		bubble_sort(0, 3);			//4つの値の中で、最も大きい値を求める
		
		switch (adk[0]){
			case 0:					//前頭部
			khr_2Bmotion(FORWARD);	//前進モーション再生
			break;
			
			case 1:					//右肩
			khr_2Bmotion(RIGHTSTEP);//右ステップ再生
			break;
			
			case 2:					//左肩
			khr_2Bmotion(LEFTSTEP);	//左ステップ再生
			break;

			case 3:					//後頭部
			khr_2Bmotion(BACKWARD);	//後進モーション再生
			break;
		}
	}
}