改良版(仮)
Dependents: harurobo1006 harurobo_1026
Fork of EC by
SpeedControl Class Reference
速度制御用ライブラリ More...
#include <SpeedController.h>
Inherits Ec.
Public Member Functions | |
SpeedControl (PinName signalA, PinName signalB, PinName signalZ, int s, double t, PinName pwm_F, PinName pwm_B) | |
constractorの定義 | |
void | Sc (double target_omega) |
速度制御関数、引数は目標速度 | |
void | setPDparam (double p, double d) |
PIDパラメータ設定関数 | |
void | setDOconstant (double c) |
角速度・duty比変換定数(C)の設定関数 | |
void | stop () |
モーター停止用関数 | |
void | turnF (double duty) |
モーター正回転用関数 | |
void | turnB (double duty) |
モーター逆回転用関数 | |
void | ScZ2 (double target_RPM) |
Z相を用いた速度制御 | |
virtual void | reset () |
エンコーダを初期状態に戻す関数 countやomegaの値を0にする | |
int | getCount () |
countの値を返す関数(int型) | |
void | CalOmega () |
omega(角速度 rad/s)の値を計算する関数 | |
double | getOmega () |
omega(角速度 rad/s)の値を返す関数(double型) | |
double | getPreCount () |
四倍精度のcountの値を返す関数(double型) | |
void | setTime (double t) |
角速度計算の間隔dtを決めることができる(デフォルトは0.05秒) | |
int | getRev () |
(Z相を使用する場合)回転回数を返す関数(int型) | |
double | getRPM () |
(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型) | |
double | getDistance_mm () |
先にsetDiameter_mmをしないと正しい値とならない | |
void | setDiameter_mm (double diameter_mm) |
void | setGearRate (double gear_rate) |
Data Fields | |
double | C |
duty比を角速度に変換させるための定数 | |
double | duty |
出力duty比 |
Detailed Description
速度制御用ライブラリ
サンプルプログラム
//サンプルプログラム 差動二輪駆動ロボットを想定 //直進・後退・右旋回・左旋回が可能 #include "mbed.h" #include "SpeedController.h" #define BASIC_SPEED 20 //モーターはこの角速度で駆動させる SpeedControl motorR(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //右輪 SpeedControl motorL(PF_1,PA_11,NC,1048,0.05,PA_9,PA_10); //左輪 Ticker motor_tick; //角速度計算用ticker Serial pc(USBTX,USBRX); void calOmega() //角速度計算関数 { motorR.CalOmega(); motorL.CalOmega(); } int main(void){ motor_tick.attach(&calOmega,0.05); //Cの値を設定 motorR.setDOconstant(46.3); motorL.setDOconstant(47.2); //pd係数を設定 motorR.setPDparam(0.1,0); motorR.setPDparam(0.12,0); int kai=0; double target_R=0,target_L=0; char status[5][10]={"STOP","FORWARD","BACKWARD","RIGHT","LEFT"}; int status_num=0; //初期状態は停止 while(1) { motorR.Sc(target_R); motorL.Sc(target_L); //target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う if(kai>=500) { pc.printf("status=%s omega_R=%f omega_L=%f \r\n",status[status_num],motorR.getOmega(),motorL.getOmega()); //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 kai=0; } if(pc.readable()) { char sel=pc.getc(); switch (sel) { case 's': //停止 motorR.stop(); motorL.stop(); target_R=0; target_L=0; status_num=0; break; case 'i': //前進 target_R=BASIC_SPEED; target_L=BASIC_SPEED; status_num=1; break; case 'm': //後退 target_R=(-1)*BASIC_SPEED; target_L=(-1)*BASIC_SPEED; status_num=2; break; case 'k': //右旋回 motorR.stop(); target_R=0; target_L=BASIC_SPEED; status_num=3; break; case 'j': //左旋回 motorL.stop(); target_R=BASIC_SPEED; target_L=0; status_num=4; break; default: break; } if(sel=='q'){ break; //qを押したら左右のモーターを停止 } } kai++; } motorR.stop(); motorL.stop(); return 0; }
基本的な速度制御の実用までの流れ
1. モーター・エンコーダ・モータードライバ・マイコン・バッテリーを適切につなげる
接続のイメージ↓
https://www.fastpic.jp/images.php?file=0391548523.png
注意!!! リポ用のスイッチと電圧計を必ずつけること!!!
/
2. 実際にモーターを回転させてみて不備なく動作するか確認する
確認事項
・モーターが回るか、duty比によって加速・減速するか
・エンコーダの値が取れているか
・モーターの回転方向とエンコーダの加算減算が対応しているか (プログラム内で定義した正・負回転とエンコーダの増減が対応しているかということ)
などなど。コードは下の一つ目のものを使えばいい
/
3. Cの値を見つける
このライブラリでは、「モーターにかかる電圧(duty比)と回転速度はある程度比例の関係にあるだろう」という考え方をしている(もちろん実際のモデルはもっと複雑である)。
このライブラリにおける速度制御のアルゴリズムとしては、あらかじめ求めたこの比例係数を用いて、目標角速度からだいたいのduty比を算出し、そこからpd制御をかけるという形をとっている。
Cとはこの比例係数のことである。一応式としてあらわすと、
(回転速度)=C×(duty比)
となる。このCの値はモーターやギアユニットによって固有の値であり、それぞれのモーターにおいて求める必要がある。
具体的なこの係数の求め方は、duty比を0.05くらいずつで変化させ、その時のduty比と回転角速度を記録し、excelからグラフの傾きを求めればよい。
こんな感じになるよっていう画像↓
https://www.fastpic.jp/images.php?file=2416798781.gif
コードは下の一つ目のものを使えばいい
/
4. pd係数を見つける
上記の通り、最終的にはpd制御(精確にはi+pd制御)によって目標角速度に収束させる。
ここではそのpd制御の操作量を決定する係数を見つける。これも実験的に求めるほかなく、値を変えて挙動を見てを繰り返して最適な係数を見つける。
値の評価は、目標に達するまでの速さ、オーバーシュートの少なさ、収束後の値の振動の小ささなどである。
重要度的にはp制御>d制御(場合によってはd制御が不必要なことも)なので、まずdの係数を0にした状態で先にpの係数を見つける。
コードは下の二つ目のものを使えばいい
//2,3で使えるサンプルコード //入力はTera Termで行う #include "mbed.h" #include "SpeedController.h" SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); Serial pc(USBTX,USBRX); Ticker motor_tick; void calOmega(){ motor.CalOmega(); //角速度計算用 } int main(void){ motor_tick.attach(calOmega,0.05); int loop_time=0; float in_duty=0; bool print=false; while(1){ loop_time++; if(in_duty>0) motor.turnF(in_duty); //in_dutyが正のとき正回転(このプログラムではPA_8で駆動) else motor.turnB((-1)*in_duty); //in_dutyが負のとき逆回転(このプログラムではPB_5で駆動) if(pc.readable()){ char sel=pc.getc(); if(sel=='s'){ motor.stop(); } else if(sel=='f'){ in_duty+=(float)0.05; //fを押すとduty比がo.o5ずつあがる pc.printf("duty=%f\r\n",in_duty); } else if(sel=='b'){ in_duty-=(float)0.05; //bを押すとduty比がo.o5ずつさがる pc.printf("duty=%f\r\n",in_duty); } else if(sel=='p'){ print=!print; //pを押すと表示を停止/再開する } } if(loop_time%1000==0){ if(print) pc.printf("count=%d omega=%f dutyF=%f dutyB=%f\r\n",motor.getCount(),motor.getOmega(),(double)motor.pwm_F_,(double)motor.pwm_B_); } //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示 } }
//PDの係数を探したいときに使えるサンプル //入力はTera Termで行う #include "mbed.h" #include "SpeedController.h" #define TARGET_OMEGA 20 //目標角速度を20rad/sに設定 SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定 Ticker motor_tick; //角速度計算用ticker Serial pc(USBTX,USBRX); void calOmega() //角速度計算関数 { motor.CalOmega(); } int main() { motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 motor.setDOconstant(46.3); //求めたCの値を設定 int kai=0; double target_omega=0; double P=0.0,D=0.0; while(1) { motor.Sc(target_omega); //速度制御のコア関数 if(kai>=500) { pc.printf("count=%d omega=%f target_omega=%f dutyF=%f dutyB=%f P=%f D=%f\r\n",motor.getCount(),motor.getOmega(),target_omega,(double)motor.pwm_F_,(double)motor.pwm_B_,P,D); //エンコーダのカウント、角速度、目標角速度、正方向出力duty比、負方向出力duty比、Pの係数、Dの係数を表示 kai=0; } //係数探しの流れとしては、係数変更→速度制御開始→収束具合を観察・比較→モーター停止の繰り返し if(pc.readable()) { char sel=pc.getc(); switch (sel) { case 'k': //kでpの係数に0.01加算 P+=0.01; break; case 'm': //mでpの係数に0.01減算 P-=0.01; break; case 'j': //jでdの係数に0.01加算 D+=0.01; break; case 'n': //nでdの係数に0.01減算 D-=0.01; break; case 's': motor.stop(); //sでモーター停止 target_omega=0; break; case 'x': target_omega=TARGET_OMEGA; //xで速度制御開始 break; default: break; } if(sel=='q'){ break; //qを押したら終了 } motor.setPDparam(P,D); //変更したパラメータをセット } kai++; } motor1.stop(); }
Definition at line 282 of file SpeedController.h.
Constructor & Destructor Documentation
SpeedControl | ( | PinName | signalA, |
PinName | signalB, | ||
PinName | signalZ, | ||
int | s, | ||
double | t, | ||
PinName | pwm_F, | ||
PinName | pwm_B | ||
) |
constractorの定義
- Parameters:
-
signalA エンコーダA相のピン名 signalB エンコーダB相のピン名 signalZ エンコーダZ相のピン名 s エンコーダの分解能 t 角速度計算の間隔 pwm_F motorを正転させるpwmピン名 pwm_B motorを後転させるpwmピン名 モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
Definition at line 8 of file SpeedController.cpp.
Member Function Documentation
void CalOmega | ( | ) | [inherited] |
omega(角速度 rad/s)の値を計算する関数
CAUTION
CalOmega関数は、一定時間ごと(dtごと)に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。
#include "mbed.h" #include "EC.h" //ライブラリをインクルード Ec Ec1(PA_0,PA_1,NC,1024,0.05); //分解能1024、計算間隔0.05秒に設定、Z相は使わない Ticker ticker; DigitalIn button(USER_BUTTON); Serial pc(USBTX,USBRX); void calOmega(){ Ec1.CalOmega(); } int main(){ ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 while(1){ pc.printf(" count1=%d ",Ec1.getCount()); pc.printf(" omega1=%f\r\n ",Ec1.getOmega()); if(pc.readable()) { char sel=pc.getc(); if(sel=='r') Ec1.reset(); //rを押したらリセット } } }
double getDistance_mm | ( | ) | [inherited] |
double getOmega | ( | ) | [inherited] |
double getPreCount | ( | ) | [inherited] |
double getRPM | ( | ) | [inherited] |
(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型)
SAMPLE
z相を使う場合のプログラムの例
#include "mbed.h" #include "EC.h" //ライブラリをインクルード Ec Ec1(NC,NC,PA_0,1024,0.05); //A相B相が不必要な場合も、このようにNCと入れればよい DigitalIn button(USER_BUTTON); Serial pc(USBTX,USBRX); int main(){ while(1){ pc.printf(" rev1=%d ",Ec1.getRev()); pc.printf(" rpm1=%f\r\n ",Ec1.getRPM()); if(!button) Ec1.reset(); //USERボタンを押したらリセット } }
void reset | ( | ) | [virtual] |
エンコーダを初期状態に戻す関数 countやomegaの値を0にする
Reimplemented from Ec.
Definition at line 161 of file SpeedController.cpp.
void Sc | ( | double | target_omega ) |
速度制御関数、引数は目標速度
この目標角加速度になるようにモーターを回転させることができる
負の数を代入すれば逆回転することができる
出力できるduty比は最大で0.5までに設定してある
- Parameters:
-
target_omega 目標角速度
CAUTION(printfについて)
上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
(どんなプログラムにも言えるが、)そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
Definition at line 18 of file SpeedController.cpp.
void ScZ2 | ( | double | target_RPM ) |
void setDiameter_mm | ( | double | diameter_mm ) | [inherited] |
void setDOconstant | ( | double | c ) |
角速度・duty比変換定数(C)の設定関数
文字通りである
- Parameters:
-
c duty比を角速度に変換させるための定数
Definition at line 157 of file SpeedController.cpp.
void setGearRate | ( | double | gear_rate ) | [inherited] |
void setPDparam | ( | double | p, |
double | d | ||
) |
PIDパラメータ設定関数
引数はそれぞれp,dパラメータ
デフォルトは全部0に設定してある
パラメーター値は実験的に頑張って求めるしかないです
- Parameters:
-
p p制御パラメータ d d制御パラメータ
Definition at line 151 of file SpeedController.cpp.
void setTime | ( | double | t ) | [inherited] |
void stop | ( | ) |
モーター停止用関数
Definition at line 168 of file SpeedController.cpp.
void turnB | ( | double | duty ) |
void turnF | ( | double | duty ) |
Field Documentation
double C |
duty比を角速度に変換させるための定数
使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める
多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない
デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果
Definition at line 319 of file SpeedController.h.
double duty |
出力duty比
Definition at line 375 of file SpeedController.h.
Generated on Thu Aug 4 2022 11:33:19 by 1.7.2