/***********************************************************************/
/*                                                                     */
/*  FILE        :GyroFeedback.c                                        */
/*  DATE        :Wed, Apr 23, 2008                                     */
/*  DESCRIPTION :main program file.                                    */
/*  CPU GROUP   :26                                                    */
/*                                                                     */
/*  This file is generated by Renesas Project Generator (Ver.4.8).     */
/*                                                                     */
/***********************************************************************/
/*                                                                     */
/*  プログラム説明                                                     */
/*                                                                     */
/*  このプログラムは、SISO JUNK STUDIO/SISO-LABの2008.04.23に公開された*/
/*  RPU-10講座／第十三章 ジャイロセンサを使ってサーボへフィードバック  */
/*  のプログラムをＫＯＮＤＯのKCB-1用に修正したものです。              */
/*                                                                     */
/*  環境　KCB-1、HEW                                                   */
/*                                                                     */
/*  説明  ビルドされた本プログラムをKCB-1へ転送後、パソコン側でターミ  */
/*        ナルソフトを使って通信速度115200bps、８ビット、EVENパリティ  */
/*        で通信ポートを開いてください。                               */
/*        その後KCB-1を再起動するとプログラムがスタートし、キーボード  */
/*        入力待ちになります。                                         */
/*        ・aを入力すると、モーション開始                              */
/*        ・bを入力すると、モーション停止                              */
/*                                                                     */
/***********************************************************************/

#include <ad.h>
#include <com.h>
#include <sio.h>
#include <led.h>
#include <timer.h>
#include <string.h>

//  定数定義
#define     SERVO_MAX           3           //  制御対象サーボ数

//  ジャイロの設定値について
//      それぞれの環境で異なると思いますので、適切な値を設定してください。
#define     GYRO_ORIGIN         320L        //  ジャイロ取り込み中心値
#define     GYRO_WIDTH          110L        //  ジャイロ振れ幅値
#define     GYRO_LEVEL          100L        //  ジャイロ振れ幅に対して角度に反映する量

//  動作データ構造定義
//      サーボに角度を与えるための情報を保持する構造体で、動作時間と目標角度で
//      構成しています。
typedef struct {
	unsigned short  usTime;                 //  動作時間（20ms単位）
	short           asAngle[SERVO_MAX];     //  目標角度
} TPOSE;

//  動作データ設定
const TPOSE GPposeList[] = {
	//  TIME    	SERVO00	SERVO01	SERVO02
	{   50,     {   9500,	9500,	9500	}},
	{   50,     {   7500,   7500,   5500    }},
	{   50,     {   7500,   5500,   5500    }},
	{   50,     {   5500,   5500,   5500    }},
	{   25,     {   7500,   5500,   7500    }},
	{   0,      {   0,      0,      0       }}
};

//  サーボ動作管理
typedef struct  {
    unsigned short  usReqTime;              //  動作要求時間（20ms単位）
    unsigned short  usCurTime;              //  制御時間（20ms単位）
    short           asReqAngle[SERVO_MAX];  //  要求角度
    short           asCurAngle[SERVO_MAX];  //  現在指示角度
    short           asBgnAngle[SERVO_MAX];  //  処理開始時角度
} TSRVM;
//  サーボ動作管理データの設定(RAM)
TSRVM   GRservoMgr;

//
//  サーボ動作管理
//      要求された時間、角度により、分割した角度指示をサーボに行います。
//
void    moveServo( void ){
    unsigned char   ucID;
    unsigned short  usGyroX, usGyroY;       //  ジャイロセンサ用変数
    short           asGyroFb[SERVO_MAX];    //  ジャイロフィードバック計算結果

    //  新規にデータ登録されたかどうかをチェックする。
    if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime == 0 )){
        //  新規にデータ登録された場合、各データを初期化する。
        for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
            GRservoMgr.asBgnAngle[ucID] = GRservoMgr.asCurAngle[ucID];
            GRservoMgr.usCurTime = 0;
        }
        //  制御時間をインクリメントします。
        GRservoMgr.usCurTime++;
    }

    //  サーボ角度補間処理
    if(( GRservoMgr.usReqTime != 0 )&&( GRservoMgr.usCurTime != 0 )){
        for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
            //  補間計算
            //      計算の順番に注意してください。また、桁あふれしないようにlong型に
            //      castして計算しています。
            GRservoMgr.asCurAngle[ucID] = GRservoMgr.asBgnAngle[ucID] +
                    (short)((long)( GRservoMgr.asReqAngle[ucID] - GRservoMgr.asBgnAngle[ucID] ) *
                    (long)GRservoMgr.usCurTime / (long)GRservoMgr.usReqTime );
        }
        //  制御時間をインクリメントします。
        GRservoMgr.usCurTime++;
        if( GRservoMgr.usReqTime < GRservoMgr.usCurTime ){
            //  終了処理
            GRservoMgr.usReqTime = 0;
            GRservoMgr.usCurTime = 0;
        }
    }
 
    //  ジャイロフィードバック値の計算
    //      ジャイロフィードバック量を取り込み、値を角度補正値に変換します。例によって
    //      桁落ちしないように計算順序を変更し、桁あふれしないようにlong型にしています。
    //      サーボ00にX軸ジャイロ、サーボ01にY軸ジャイロ、サーボ02はフィードバック無し
    //      としています。

	//  ジャイロの値を取得
	usGyroX = ad1;
	usGyroY = ad2;

	//asGyroFb[0] = ((long)usGyroX - GYRO_ORIGIN ) * GYRO_LEVEL / GYRO_WIDTH;
	//asGyroFb[1] = ((long)usGyroY - GYRO_ORIGIN ) * GYRO_LEVEL / GYRO_WIDTH;
    asGyroFb[0] = (long)usGyroX;
    asGyroFb[1] = (long)usGyroY;	
    asGyroFb[2] = 0;
 
    //  サーボへの角度指示
    //      計算結果（計算していない場合は前のままの値）をサーボに指示します。
    for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
    	sio1_set_pos(ucID, GRservoMgr.asCurAngle[ucID] + asGyroFb[ucID]);		
		wait(1000);
	}
}


void main(void)
{
	short			sStep;                  //  ステップ
    unsigned char   ucID;                   //  サーボID
	
	cpu_init ();							// CPU初期化
	com_open(BR115200, 8, 1, PARITY_EVEN);	// COMをオープン
	sio1_init ();							// SIO1初期化
 	ad_repeat_sweep0(); // AD初期化
	ad_start();			// AD開始

	ledgrn_on();		// LED緑ON
 
    sStep = -1;										//  -1で停止と扱うので、事前初期化。
	memset( &GRservoMgr, 0x00, sizeof( TSRVM ));	//  サーボ管理データの初期化

	wait(200000);

	//  初期ポジション
	GRservoMgr.usReqTime = 100;
	GRservoMgr.usCurTime = 0;
	for(ucID=0; ucID<SERVO_MAX; ucID++){
		GRservoMgr.asReqAngle[ucID] = 7500;
		GRservoMgr.asCurAngle[ucID] = sio1_get_pos(ucID);
		GRservoMgr.asBgnAngle[ucID] = GRservoMgr.asCurAngle[ucID];
	}

    //  起動メッセージの表示
	printf("MOVE 3 SERVOS LIKE MOTION PLAY\n");
	printf("PLEASE INPUT KEY a=START, b=STOP.\n");
 
    //  メインループ
    //      20msec毎にループします。
	while(1){
		ledred_on();	// LED赤ON
 
		//  キー入力の受付
		if(ri_u1c1 != 0){		//  受信バッファーチェック
			switch( com_getchar()){
			case 'a':                           //  動作開始
				//  sStepを0にする（-1以外）ことで動作開始とします。
				sStep = 0;
				printf("START\n");
				break;
			case 'b':                           //  ストップ
				//  ステップ動作を停止する。
				sStep = -1;
				//  サーボ制御処理も新たな計算を停止させる。
				GRservoMgr.usReqTime = 0;
				GRservoMgr.usCurTime = 0;
				printf("STOP\n");
				break;
			}
		}
 
        //  サーボ指示処理
        //      sStepが-1以外でかつ、sTimeが０のときに新しい角度指示を行います。
        //      角度指示後、sTimeは20msec毎に減算されていき、０になったとき、
        //      また新しい角度指示を行います。
        if( sStep != -1 ){
            if( GRservoMgr.usReqTime == 0 ){
                if( GPposeList[sStep].usTime != 0 ){
					//  指示情報表示
					printf("STEP=%d ", sStep);
                    //  角度指示
                    GRservoMgr.usReqTime = GPposeList[sStep].usTime;					
                    printf("ReqTime=%d ", GRservoMgr.usReqTime);
					for( ucID = 0; ucID < SERVO_MAX; ucID++ ){
                        GRservoMgr.asReqAngle[ucID] = GPposeList[sStep].asAngle[ucID];						
						printf("ReqAngle[%d]=%d ", ucID, GRservoMgr.asReqAngle[ucID]);
                    }
					printf("\n");
                    //  nStepをインクリメントし、次回指示にそなえます。
                    sStep++;
                }
                else{
                    //  再生終了処理
                    //      設定データの動作時間が０なので、動作を完了します。
                    sStep = -1;
                }
            }
        }
 
        //  サーボ動作管理と指示
        //      この処理は常時20msec毎に呼び出され、角度指示を行います。
        moveServo();
 
		//  20msec待ちます。
		//
		//  100msec以下の待ち時間t[μSec]時のカウント値wは、
		//  w = (t - 3.5386) / 1.5480    (KCB-1 SDK Manual 参照)
		//  w = (20000 - 3.5386) / 1.5480 = 12917
		wait(12917);
    }
}