/**************************************************
 サーボＳ字加減速制御             RIKIYA 2005.04.03 
                                        srv_ctrl1.C 
H8-3048/F                                          
ITU0を利用したPWMによるラジコンサーボのＳ字加減速
/**************************************************/

#include <3048f.h>

int  srv_ctrl(long start,long stop,long m_time);
void pwm_wait(void);

/* メイン関数 ********************************************/

int main(void){

     ITU0.TCR.BYTE = 0x23;   // GRAでカウンタクリア φ/8
     ITU0.GRA = 40000;	     // GRAを40000に設定 
     ITU0.GRB = 1500;        // GRBを1500に設定 
     ITU.TMDR.BIT.PWM0 = 1;  // ITU0 PWMモード 
     ITU.TSTR.BIT.STR0 = 1;  // カウント開始 

     while(1){               // Ｓ字加減速動作の実行
          while(srv_ctrl(1500,4500,1500) != 1){};
		  while(srv_ctrl(4500,2500,750)  != 1){};
  		  while(srv_ctrl(2500,1500,500)  != 1){};
     }
}

/* サーボＳ字加減速制御 *********************************/
// start : 開始位置パラメータ
// stop  : 終了位置パラメータ
// m_time: 移動時間 mS
// return -1 : 入力値エラー
// return  1 : 正常終了

int srv_ctrl(long start,long stop,long m_time){

#define PWM_PITCH 20;        // PWM周期の指定

long with,time;
float a;

     // 引数チェック
     if (m_time <= 0) return -1;

     // 加速度計算
     a = 2*((float)stop-(float)start)/((float)m_time*(float)m_time);

     // 絶対値化（符号クリア）
     if(a < 0){ a *= -1; }
     ITU0.TCNT = 0;
	 time = 0;

/////////////////////////////////////////////////////////
     // 正方向移動
     if((stop-start) > 0){

     // 正方向加速制御
		 do{
		      with = a * time * time + start;
			  ITU0.GRB = with;
			  pwm_wait();
			  time += PWM_PITCH;
         }while(time < (m_time/2));

     // 正方向減速制御
         do{
		      with = -1 * a * (m_time-time) * (m_time-time) + stop;
			  ITU0.GRB = with;
			  pwm_wait();
			  time += PWM_PITCH;
         }while(time <= m_time);
         return 1;
     }
/////////////////////////////////////////////////////////
	 // 負方向移動
     else if((stop-start) < 0){
 
     // 負方向加速制御
		 do{
		      with = -1 * a * time * time + start;
			  ITU0.GRB = with;
			  pwm_wait();
			  time += PWM_PITCH;
        }while(time < (m_time/2));

     // 負方向減速制御
         do{
		      with = a * (m_time-time) * (m_time-time) + stop;
			  ITU0.GRB = with;
    		  pwm_wait();
			  time += PWM_PITCH;
         }while(time <= m_time);
         return 1;
     }
     return -1;
}

/* PWM終了タイミング検出 ********************************/

void pwm_wait(void){
	do{	                    /* TCNT = GRBなるまで待つ*/
	}while(ITU0.TSR.BIT.IMFB == 0);
	ITU0.TSR.BIT.IMFB = 0;  /* 検知フラグを戻して再開 */
    return;
}