ボリュームでサーボ制御

H8_adv_ADsarvo2in1

 #include<3664.h>
#include

struct sarvo{
    float ang_max;
    float ang_min;
};

int GRB;

void ini_AD(void){
   AD.ADCSR.BYTE = 0x38;     /* 0011 1000*/
                            /* A/D変換開始 */
                            /* スキャンモード */
                            /* 高速変換 */
                           /* AN0 */ 
}

int AD_Vlm(int div){
/**************************************
 　引数 ： div (VLキザミ値)
 　　　    4,8,16,32,64,128,256 で指定
        
 　返値 ： 0～div　を返す
           VLﾎﾞﾘｭｰﾑの回転に応じて
 **************************************/       

    int i;
    switch(div){
        case  4:    i=14; break;
        case  8:    i=13; break;
        case 16:    i=12; break;
        case 32:    i=11; break;
        case 64:    i=10; break;
        case 128:   i=9 ; break;
        case 256:   i=8;  break;
       default:     i=14; 
    }

   while(AD.ADCSR.BIT.ADF==0);      // 変換結果がでるまで待つ

   return( (int)( AD.ADDRA>> i ) );   //　ビットシフト(i桁ずらす)

//    AD.ADCSR.BIT.ADF=0;           /* 終了ﾌﾗｸﾞを０に戻す  */
}

void ini_DrvSarvo2M(){

  TW.TMRW.BYTE=0x01;      // FTIOB端子をPWM出力に設定 
  TW.TCRW.BYTE=0xB2;      // ﾏｯﾁＡでカウンタクリア、内部クロックの1/8( 2MHz )　
  			//FTIOBの初期値は1

  TW.TCNT=0x0000;         // TCNの初期化
  TW.GRA=20000;            // 20mS (50Hz)
  TW.GRB=3000;            // 1.5mS 
}

void DrvSarvo2M(struct sarvo sv,int inp,int div){
/**********************************
機能　＝　引数inpの数字に応じてサーボを回転させる
 引数：struct sarvo sv　　サーボ上下限値(ms)
 　　：div　スケール
 　　：inp　入力数値
 返値：なし
 動作：サーボ回転
***********************************/

    int GRout;
    long GRmax;
    long GRmin;
    long p;

//範囲外処理 
     if(sv.ang_max > 2.3 || sv.ang_min < 0.7) {return;} //何もせずに返る
     
    GRmax=(int)2000*sv.ang_max;
    GRmin=(int)2000*sv.ang_min;

    GRmax=GRmax*100;//少数演算用100倍long
    GRmin=GRmin*100;//少数演算用100倍long
    p=(  ((GRmax-GRmin)/div)*inp ); //

    GRout=(p + GRmin )/100;

//範囲外処理 
    if(GRout > 4600 || GRout < 1400) {return;}

    GRB=GRout;   //デバッグ出力
    TW.GRB=GRout;//パルス発生出力
}


void main(void)
{

  DI;
  
    LCD_init( 16 ); 
    ini_AD();
    ini_DrvSarvo2M();

  TW.TMRW.BIT.CTS=1;      // TCNTカウンタスタート 
  EI;

  while(1){

  struct sarvo s3003;
    s3003.ang_max=2.0;
    s3003.ang_min=1.0;    

 DrvSarvo2M(s3003,  AD_Vlm( 64 ),  63);

        //LCD表示                   
        LCD_locate(1,2);
        LCD_dataout(GRB );
  }

}