標準動作プログラム PWM モジュール取扱い説明書

沼津高専電子制御工学科

標準動作プログラム PWM モジュール取扱い説明書
MIRSSTND-MANU-0003

改訂記録

版数

作成日

作成者

承認

改訂内容

A01

2004.04.28

瀬川

牛丸

初版

始めに
インストール手順
アプリケーションインターフェース(FIFO)
RT-タスクモジュール
行動制御プログラム中で使用する制御関数
使用上の注意
動作試験

1.始めに

本ドキュメントは、MIRSSTNDの行動制御プログラム中で使用するPWM・ロータリーエンコーダ制御用関数の機能と、行動制御プログラム/RT-task間のInterfaceについて記したものである。

2.インストール手順

本モジュールをRT-カーネルに組み込む方法を下に示す。

ファイル（pwm_enc_module2.tar.gz）をダウンロードする。

ファイルを展開する。
>tar xzvf pwm_enc_module.tar.gz
ここで展開されるファイルの一覧は、以下の13個である。

both.c           ：タスク制御用
enc_test.c       ：エンコーダのテストプログラム
encoder.c　　    ：エンコーダのカウントを読み込み、速度と距離の算出を行う。
encoder2.c       ：encoder.cに同じ(100msごとの回転角度を返す)
encoder3.c       ：encoder.cに同じ(回転角度の合計を返す)
pwm.c            ：pwm制御用
pwm_enc_module2.c：RTタスクモジュール
pwm_stop.c       ：タイヤの停止プログラム
system_check.c   ：pwm.cのAPIのチェックプログラム
pwm_enc.h        ：タイヤやタスクのパラメータなどを定義
pwm_module.h     ：RTタスクモジュールの制御用パラメータの定義
rtl.mk           ：コンパイルオプションの定義
Makefile         ：メイクファイル

すべてコンパイルする。
>make
モジュールを組み込む。
>insmod pwm_enc_module2.o
モジュールが組み込まれたことを確認する。
>lsmod

モジュールの削除は下記のコマンドを使用して行う。
>rmmod pwm_enc_module2

コンパイルを行って作成したファイルの削除は下記のコマンドを使用して行う。
>make clean

Makefileについて

必要があれば下記のようにMakefileを書き換える。
例えば、both.cで定義されている関数を中に含むプログラムtest.cをコンパイルして、testという名前の実行ファイルを作成したい場合は、

both.o: both.c
    $(CC) -O2 -Wall -c both.c
test: test.c both.o 
    $(CC) ${INCLUDE} -O2 -Wall both.o test.c -o test

また、一番上のallのところにもコンパイルしてできるファイル名を書き加えておく。(上の例の場合はboth.oとtestを書き加える。)

3.アプリケーションインターフェイス(FIFO)

本モジュールと行動制御プログラムとのインターフェースには、下記の３つのFIFOを使用する。

FIFO3（ロータリーエンコーダのカウント値用）

左右のロータリーエンコーダのカウント値が書き込まれる。
データの送受の向きは、RT-タスク ---> 行動制御プログラム

FIFO4（PWM指定用）

左右のPWMデータ,実行モードが書き込まれる。
データの送受の向きは、行動制御プログラム ---> RT-タスク

FIFO5（RT-タスク制御用）

START、STOP、RESTARTの指令とRT-タスクの実行周期が書き込まれる。
データの送受の向きは、行動制御プログラム ---> RT-タスク

FIFOの使用例

ユーザーがFIFOをオープンする(FIFOの読み書きをする時に必要-pwm.c,both.c,encoder.cで使用)
fifo4(/dev/rtf4)を読み書き権限で開くときを下に示す。
```
int fd4;
if((fd4 = open("/dev/rtf4", O_WRONLY)) < 0){
    fprintf(stderr, "Error opening /dev/rtf4\n");
    return 0;
}
```
また、読み込みだけならばopen()の第二引数をO_RDONLYとすればよい。
ユーザーがFIFOに書き込む(RTタスクにデータを渡すときに必要-pwm.c,both.cで使用)
ここでpwmという構造体にint型のleft,rightという値を代入し、それをFIFO4に書き込むという状況を考える。
fd4はすでに上でR/Wモードでopenされているとすると、以下のようにすればよい。
```
int left,right;
struct pwm_speed_data pwm;
pwm.l_speed = left;
pwm.r_speed = right;
if(write(fd4, &pwm, sizeof(pwm)) < 0){
    fprintf(stderr, "Can't send a data to RT-task\n");
    return 0;
}
close(fd4);
```
write()で書き込んだ後はclose()で必ず閉じるようにする。
このopen()とwrite()を使えば、pwm.cからRTタスクモジュールへFIFOを介してデータを渡すことができる。
ユーザーがFIFOから読み込む(FIFOにデータが書き込まれたらFIFOからデータを取り出す時に必要-encoder.cで使用)
FIFO3がすでにopenされているとすると、tv_usecで指定した100msの間select()により、FIFOの変化を監視する。
FIFOにデータが書き込まれた場合select()の戻り値は0以上になる。read()により、構造体countに値を代入する。それをまとめると以下のようになる。
```
fd_set rfds;
struct timeval tv;
struct count_data count;
int retval1, retval2;

FD_ZERO(&rfds);
FD_SET(fd3, &rfds);
tv.tv_sec = 0;
tv.tv_usec = 100000;

retval1 = select(FD_SETSIZE, &rfds, NULL, NULL, &tv);
if(retval1 > 0){
    while(1){
        read(fd1, &count, sizeof(count));
        retval2 = select(FD_SETSIZE, &rfds, NULL, NULL, &tv);
        if(retval2 == 0) break;
    }
}
```
RTタスクがFIFOから読み込む
ユーザーがFIFOに書き込むのところで書き込んだデータをRTタスクで読み取る場合は以下のようにrtf_get()を使う。
```
int fifo4=4;
int pwm_data_l1,pwm_data_r1;
struct pwm_speed_data pwm;
while((err = rtf_get(fifo4, &pwm, sizeof(pwm))) == sizeof(pwm)){
    pwm_data_l1 = pwm.l_speed;
    pwm_data_r1 = pwm.r_speed;
}
```
これで、先ほどwrite()で書き込まれたpwm.l_sppedとpwm.r_speedをRTタスク側の変数に代入することができる。
RTタスクがFIFOに書き込む
RTタスクがFIFOにデータを書き込む場合はrtf_put()を使用する。
```
int fifo3=3;
struct count_data count;
rtf_put(fifo3, &count, sizeof(count));
```
これでcountという構造体のデータがFIFOに書き込まれる。

4.RT-タスクモジュール

モジュールソースファイル：pwm_enc_module2.c

定義されているモード：DUTY_MODE, PID_MODE, LQR_MODE

DUTY_MODE	指定したDUTY比でモータを回す。
PID_MODE	指定した角度または距離だけ、モータを回し、その値に達したら止める。
LQR_MODE	指定した角度または距離を、指定した時間で移動する。

PID_MODEの制御
ロータリエンコーダのカウンタ値を読み取り、その値に近づいたら、モータに送るduty比を小さくする比例制御を行っている。その係数は、左右それぞれ、Kpl, Kprで与え、制御を終了する閾値（エンコーダのパルスベース）をVminで与える。また、モータやMIRSの制動トルク分をduty比ベースで、vshiftとして与える。
LQR_MODEの制御
状態空間モデルを用いた制御を行っている。与えられた実行周期で目標値と出力を計算（ロータリーエンコーダのカウント値から）し、その偏差（タイヤの回転角、ラジアン）がerrmaxより小さくなったら停止する。PID同様左右のDuty比のシフトはそれぞれ、drshift,dlshiftで与え、状態空間モデルの係数行列はAd,Bd,Cdで与えている。
モジュールの構成

void *thread_code(void *t)

メインスレッド。コマンドハンドラから渡された実行周期でPWMの制御（主にpwm.cからのデータによるモードごとの制御と、エンコーダのカウント値をFIFOに書き込む）を行う。

int my_handler(unsigned int fifo)

コマンドハンドラ。both.cでFIFOに書き込まれたスレッドの実行周期やスレッドのSTART,STOP,RESTARTの状態遷移を行う。

int init_module(void)

モジュールのロード時に最初に実行される初期化メソッド。FIFOの生成や、スレッドの生成を行う。

void cleanup_module(void)

FIFOの削除などの終了時の処理を行う。

5.行動制御プログラム中で使用する制御関数

RT-タスク制御用（both.cの中で定義されている関数）
void pwm_enco_start(int period)

ロータリーエンコーダ・PWM制御用のRT-タスクをスタートさせるための関数。実行周期を[msec]単位で引数として与える。

void pwm_enco_stop(void)

ロータリーエンコーダ・PWM制御用のRT-タスクを停止させるための関数。

void pwm_enco_restart(void)

pwm_enco_stopで停止させたRT-タスクを再スタートさせるための関数。
ロータリーエンコーダのカウントリード用1（encoder.cの中で定義されている関数）
void encoder_data(float *l_distance, float *r_distance, float *l_speed, float *r_speed, int *br)

FIFO3から左右のロータリーエンコーダのカウント値とフラグの状態を100[ms]の周期で読み込んで、左右のタイヤの移動距離/速度とフラグの状態をポインタを使って値を返す関数。
brという変数は、PID_MODE,LQR_MODEの制御の状態を示すものであり、0なら制御の途中であり、1なら制御が完了したことを示す（Duty_Modeは常に0である）。また、distanceとspeedの単位はそれぞれ[mm]と[mm/sec]である。
ロータリーエンコーダのカウントリード用2（encoder2.cの中で定義されている関数）
void encoder_data2(float *l_angle, float *r_angle, int *br)

FIFO3から左右のロータリーエンコーダのカウント値とフラグの状態100[ms]の周期で読み込み、100msごとの左右のタイヤの回転角度[deg]をポインタを使って値を返す関数。
brという変数は、PID_MODE,LQR_MODEの制御の状態を示すものであり、0なら制御の途中であり、1なら制御が完了したことを示す（Duty_Modeは常に0である）。
ロータリーエンコーダのカウントリード用3（encoder3.cの中で定義されている関数）
void encoder_data3(float *l_angle, float *r_angle, int *br)

FIFO3から左右のロータリーエンコーダのカウント値とフラグの状態100[ms]の周期で読み込み、左右のタイヤの回転角度[deg]の合計をポインタを使って値を返す関数。
brという変数は、PID_MODE,LQR_MODEの制御の状態を示すものであり、0なら制御の途中であり、1なら制御が完了したことを示す（Duty_Modeは常に0である）。
PWM制御用（pwm.cの中で定義されている関数）
void pwm_both_data(int l_duty, int r_duty)

左右のDutyを直接[-127～+127]で与える関数である。l_dutyが左のタイヤ、r_dutyが右のタイヤへのdutyである。単位は共に[-]であり、前進が＋、後進が－となっている。
MODEはDUTY_MODEである。

void pwm_straight(int l_distance, int r_distance)

P制御を行って左右のタイヤをそれぞれ指定した距離だけ進める関数である。l_distanceが左のタイヤの目標距離,r_distanceが右のタイヤの目標距離である。単位は共に[mm]であり、前進が＋、後進が－となっている。

void pwm_rotate(int angle)

P制御を行ってその場回転を行う関数である。回転角度をangle[deg]で与え、左回転が+、右回転が-となっている。

void pwm_lqr_straight(int distance, int time)

LQRモードの直進を行う関数である。移動距離をdistance[mm]、移動時間をtime[msec]で与える。前進が+、後進が-である。

void pwm_lqr_rotate(int angle, int time)

LQRモードでその場回転を行う関数である。回転角度をangle[deg]、回転時間をtime[msec]で与える。左回転が+、右回転が-である。

void pwm_lqr_circle(int radius, int angle, int time)

LQRモードで、半径rの円を描くように移動する関数である。回転半径をradius[mm]、回転角度をangle[deg]、移動時間をtime[msec]で指定する。左周りが+、右回りが-である。

6.使用上の注意

pwm.cの中で定義されている関数を使用する際は、以下のようにする必要がある。これはDUTY_MODEで0,0を書き込んだときにに、目標値やカウントをリセットする仕様になっているからである。また、これらの関数はusleep()などで間をおいて次を呼ぶ必要がある。その一例を以下に示す。

int lqr_rotate(int ang,int time){
pwm_enco_start(10);
usleep(40000);
pwm_lqr_rotate(ang,time);
usleep(40000);
count.br=0;
while(1){
        if(count.br==1) break;
        usleep(20000);
}
pwm_both_data(0,0);
usleep(40000);
pwm_enco_stop();
return 0;
}

7.動作試験

pwm_enc_module2.oを組み込む。
>insmod pwm_enc_module2.o
チェックプログラムの起動
>./system_check
```
Please Input Mode Number
1:pwm_both_data check    --> (no control) duty only (-127 to 127)
2:pwm_straight check     --> (pid control) go straight as far as reserved distance
3:pwm_rotate check       --> (pid control) rotate on the spot
4:pwm_lqr_straight check --> (lqr control) go straight as far as reserved distance
5:pwm_lqr_rotate check   --> (lqr control) rotate on the spot
6:pwm_lqr_circle check   --> (lqr control) rotate around a something
7:Exit This Menu
Select Number =
```
と表示されるので、1番から順にチェック行う。
1はDutyを-127～+127で左右それぞれ指定する。
2,3はPIDモードである。距離[mm]または角度[deg]を指定する。
4,5,6はLQRモードである。距離[mm]または角度[deg]と時間[msec]を指定する。

タイヤが意図しない方向に回る場合は、モータの±の取りつけが逆である可能性が高い。
また、ロータリーエンコーダも逆に結線されている場合、制御が終わらないという事態に陥る場合があるので注意する。

関連文書