ボリュームでラジコンサーボ制御（サーボ動作関数）

サーボ動作ブロック（モジュール）

続いてサーボ動作をさせる部分を作ります。

このサーボ動作ブロックの部分を作ることになります。

数値が入力されれば、その数値に応じてサーボが角度に回転するという機能に。

いろいろなとこから出てくる数値をサーボ動作ブロックに入力するだけでサーボ回転の制御が出来るモジュールです。

　”出てきた数値”とは、 ボリュームだったり、センサだったりと、いろいろなモジュールから出てくる数値を、入力すればサーボ回転の制御ができるような数値入力範囲にします。数値の入力範囲を可変にし、設定できるようにします。

プログラム

ラジコンサーボを動かす　で使用した設定を汎用性のある関数に改良したものです。

タイマーＷ初期化関数

　 ini_DrvSarvo2M()

これを最初に実行し、レジスタの初期設定をします。

パルス発生関数

DrvSarvo2M(　 struct sarvo　sv,　int inp,　int div)

AD_Vlm(int div)関数はあらかじめ、divの値が　4,8,・・・　と決めて、その数値にすることで入力する範囲が決まりました。

ここでDrvSarvo2Mはその範囲を自由に設定できるようにしました。

この　サーボ動作ブロックの　数値入力　の範囲を設定可能に。

渡す引数は３つ　 　
サーボの最大、最小パルス範囲設定

　struct sarvo　サーボ名
　struct sarvo s3003;

のように適当な名前で構造体変数を宣言し（ここでは　s3003）
　3003.ang_max　=　最大パルス

　s3003.ang_min　=　最小パルス　

にサーボモーターの最大、最小パルスを入れ、制御するサーボの上、下限値を設定し、s3003を　引数１番目にします。

引数３番目　divに入力する数値の最大値を入れ、0～divのスケールをつくり
引数２番目　inpにそのスケール範囲(0～div)内の目標値を渡します。

引数inpに入力された数値に応じたパルスにして出す。

関数内部で　ハードウエアリミットとして、最大パルス2.3　最小パルス1.4
　それに対応する　ＧＲＢ値も　4600、1400　として　この範囲以上のパルスは出力できないようにしています。

説明が複雑になりますが、使い方は割と簡単でなかなか使えそうな関数です。

 #include 
struct sarvo{
　　　float ang_max;
　　　float ang_min;
};

int GRB;

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の数字に応じてサーボを回転させる
引数：div:ｽｹｰﾙ　,inp:目標値　,struct sarvo:ｻｰﾎﾞ最大最小ﾊﾟﾙｽ
返値：なし
動作：サーボ回転
***********************************/

　　　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; //デバッグ出力
}


int main(void){

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

　　　DrvSarvo2M(s3003, 50, 100); //実行結果　GRB=3000
　　　printf( "GRB=%d\n", GRB );

　　　DrvSarvo2M(s3003, 100, 100); //実行結果　GRB=4000
　　　printf( "GRB=%d\n", GRB );

　　　DrvSarvo2M(s3003, 0, 63); //実行結果　GRB=2000
　　　printf( "GRB=%d\n", GRB );

　　　DrvSarvo2M(s3003, 31, 63); //実行結果　GRB=2983
　　　printf( "GRB=%d\n", GRB );
}　　　　　　　　　　
       

ＧＲＢの数値をサーボ用パルスに対応するように計算して作り出せばいいので、要するにスケール変換の要領です。

周期は　TW.GRA=20000　　10msにしてあるので、 算出したGRBをＴＷ．ＧＲＢに入れるとサーボパルスが発生します。
このタイマー初期化では　○ms×２０００＝ＧＲＢ用の数値に変換できます。

入力値をGRBに与える数値への変換後に再び、範囲外処理をしています。
変換で何か変な数値となっても、やはり範囲外の動きになるパルスを出力しないため。

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

GRout=(p + GRmin )/100;

引数div：スケール最大値　の変換処理部分。
割り算すると少数になることがありますが、小数以下２桁程度あればいいので、
ここだけのために小数型のｆｌｏａｔなどは使わず、１００倍して整数で割り算し、あとで１００で割る方法です。

本来は小数型など使わず、整数型でやるべきところです。

これはデバッガでのエミュレーション用なので　初期化関数
ini_DrvSarvo2M()　　は使用しません。

実行結果

実行してGRBがどのようになるかみると

●　サーボの上限、下限値を変更

続いて　サーボの範囲も変更してみます。

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

サーボの最大パルスを1.5ms（GRBで3000）の数値にします。これ以上の数値は出力しないはずです。

int main(void)
{


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

DrvSarvo2M(s3003, 100, 100); //実行結果　GRB=3000　（1.5ms）　
printf( "GRB=%d\n", GRB );

DrvSarvo2M(s3003, 0, 100); //実行結果　GRB=2000　　（1.0ms）
printf( "GRB=%d\n", GRB );

DrvSarvo2M(s3003, 50, 100); //実行結果　GRB=2500　（1.25ms）
printf( "GRB=%d\n", GRB );

}
       

スケールの最大、最小にすると　最大3000　最小2000　中間2500　と目標通り機能しています。

それでは次にボリュームとつなげてみます。