ラジコンサーボをパソコンで動かす

SISO_sarvos10ms_PC

#include <3664.h>

#define TIMERW_10MSEC 45535  // 10msec待つためのタイマＷ初期値
#define TIMERW_2M5SEC 60535  // 2.5msec待つためのタイマＷ初期値

_BYTE   bStep;            // サーボ制御ステップ
_WORD   awServoPos[16];   // サーボ制御位置

unsigned char txb[10], rxb[10] ;
char b[20];             //通信用

//
// タイマＷ割り込みルーチン
//
void int_timerw( void )
{
  _BYTE bSrvA, bSrvB, bSrvC, bSrvD;   // サーボ制御信号ON作業用

  if( TW.TSRW.BIT.OVF == 1 ){   // オーバーフロー発生
    // オーバーフロー発生時の処理
    TW.TSRW.BIT.OVF = 0;    // オーバーフローフラグクリア

    TW.TMRW.BIT.CTS = 0;    // タイマストップ（他のビットはデフォルト）


      TW.TCNT = TIMERW_2M5SEC;    // 2.5msec後にOVFする値設定

      // GRレジスタへの制御信号OFF時間の設定
      TW.GRA = TIMERW_2M5SEC + awServoPos[(bStep-1)+0];
      TW.GRB = TIMERW_2M5SEC + awServoPos[(bStep-1)+4];
      TW.GRC = TIMERW_2M5SEC + awServoPos[(bStep-1)+8];
      TW.GRD = TIMERW_2M5SEC + awServoPos[(bStep-1)+12];

      bSrvA = 1 << (( bStep-1 )+4 );  // 元ネタの計算
      bSrvB = 1 << (( bStep-1 ));     // 元ネタの計算
      bSrvC = 1 << (( bStep-1 ));     // 元ネタの計算
      bSrvD = 1 << (( bStep-1 )+4 );  // 元ネタの計算

      IO.PDR1.BYTE |= bSrvA;          // 信号ON！！
      IO.PDR5.BYTE |= bSrvB;          // 信号ON！！
      IO.PDR8.BYTE |= bSrvC;          // 信号ON！！
      IO.PDR8.BYTE |= bSrvD;          // 信号ON！！
    

    // ステップ情報の操作
    //   1～4:サーボ制御信号出力なので、5になっていたら
    //   数値を０に戻す。
    bStep++;
    if( bStep == 5 ){
      bStep = 1;
    }

    TW.TMRW.BIT.CTS = 1;    // タイマスタート
  }
  
  if(TW.TSRW.BIT.IMFA  == 1){   // GRA コンペアマッチ
    // GRAコンペアマッチ発生時の処理
    IO.PDR1.BYTE &= 0x0F;
    TW.TSRW.BIT.IMFA = 0;       // 割り込みフラグクリア
  }
  if(TW.TSRW.BIT.IMFB  == 1){   // GRB コンペアマッチ
    // GRBコンペアマッチ発生時の処理
    IO.PDR5.BYTE &= 0xF0;
    TW.TSRW.BIT.IMFB = 0;       // 割り込みフラグクリア
  }
  if(TW.TSRW.BIT.IMFC  == 1){   // GRC コンペアマッチ
    // GRCコンペアマッチ発生時の処理
    IO.PDR8.BYTE &= 0xF0;
    TW.TSRW.BIT.IMFC = 0;       // 割り込みフラグクリア
  }
  if(TW.TSRW.BIT.IMFD  == 1){   // GRD コンペアマッチ
    // GRDコンペアマッチ発生時の処理
    IO.PDR8.BYTE &= 0x0F;
    TW.TSRW.BIT.IMFD = 0;       // 割り込みフラグクリア
  }
}

int main()
{
  unsigned char SavNo ,Pls_w;
        
  //
  // 初期化
  //
  //通信用初期化
  SCI3_INIT(br38400, txb, sizeof(txb), rxb, sizeof(rxb));
  
  // タイマモードレジスタ
  TW.TMRW.BIT.CTS = 0;    // タイマストップ（他のビットはデフォルト）

  // タイマコントロールレジスタ
  TW.TCRW.BIT.CKS = 3;    // クロックセレクト
                          //   内部クロックをΦ８でカウントアップ

  // タイマインタラプトイネーブルレジスタ
  TW.TIERW.BIT.OVIE = 1;  // オーバーフロー割り込み有効
  TW.TIERW.BIT.IMIEA = 1; // GRA割り込み有効
  TW.TIERW.BIT.IMIEB = 1; // GRB割り込み有効
  TW.TIERW.BIT.IMIEC = 1; // GRC割り込み有効
  TW.TIERW.BIT.IMIED = 1; // GRD割り込み有効

  IO.PCR1 = 0xF0;         // ポート1のP14-17を出力設定
  IO.PCR5 = 0x0F;         // ポート5のP50-53を出力設定
  IO.PCR8 = 0xFF;         // ポート8は全部出力設定

  bStep = 1;              // ステップを０に初期化

  // データの初期化
  awServoPos[0] = 3000;  // 1.5msec   P14
  awServoPos[1] = 3200;  // 1.6msec   P15
  awServoPos[2] = 3400;  // 1.7msec   P16
  awServoPos[3] = 3600;  // 1.8msec   P17

  awServoPos[4] = 3800;  // 1.9msec   P50
  awServoPos[5] = 4000;  // 2.0msec   P51
  awServoPos[6] = 3000;  // 1.5msec   P52
  awServoPos[7] = 3200;  // 1.6msec   P53

  awServoPos[8] = 3400;  // 1.7msec   P80
  awServoPos[9] = 3600;  // 1.8msec   P81
  awServoPos[10] = 3800;  // 1.9msec   P82
  awServoPos[11] = 4000;  // 2.0msec   P83

  awServoPos[12] = 3000;  // 1.5msec   P84
  awServoPos[13] = 3200;  // 1.6msec   P85
  awServoPos[14] = 3400;  // 1.7msec   P86
  awServoPos[15] = 3600;  // 1.8msec   P87
  

  EI;                     // 割り込み許可

  TW.TCNT = TIMERW_2M5SEC;// タイマカウンタに2.5msec後にOVFする値設定
  TW.TMRW.BIT.CTS = 1;    // タイマスタート

  //文字列送信→PC
  //送信文字は　２桁＿２桁
  SCI3_OUT_STRING ("\nサーボNo（２桁）  スペース  パルス長（２桁）入力\n");//文字列送信関数
  
  
  while( 1 ){
    // せっかくなのでパソコンからキーボード入力でサーボを
    // 動かすプログラムを書いてみましょ。
        SCI3_IN_STRING (b, sizeof(b) - 1);  //文字列受信関数
        SCI3_PRINTF("[%s]\n",b);          //オウム返し表示
        
  //受信文字 10進数へ変換
    //サーボ配列No
        SavNo = ( b[0]-'0')*10;
        SavNo += ( b[1]-'0');
        
    //パルス長
        Pls_w = ( b[3]-'0')*10;
        Pls_w += ( b[4]-'0');      

        if(SavNo<16 && Pls_w<25){    //範囲処理
              awServoPos[SavNo] = Pls_w*200;
        }
  }
}

       

文字列→１０進数値の変換処理もべた書きでやってます。

//受信文字 10進数へ変換
        //サーボ配列No 
        　　　　SavNo = ( b[0]-'0')*10;
        　　　　SavNo += ( b[1]-'0');
        //パルス長
        　　　　Pls_w = ( b[3]-'0')*10;
        　　　　Pls_w += ( b[4]-'0');
              　　　　if(SavNo<16 && Pls_w<25){　　 //範囲処理
        　　　　　　　 awServoPos[SavNo] = Pls_w*200;　//ここで受取り加工した数値を配列に入力!
        　　　　}