改良版(仮)
Dependents: harurobo1006 harurobo_1026
Fork of EC by
SpeedController.h
00001 #ifndef INCLUDED_SPEEDCONTROLLER_SPEEDCONTROLLER_H 00002 #define INCLUDED_SPEEDCONTROLLER_SPEEDCONTROLLER_H 00003 #include "EC.h" 00004 /** @class SpeedControl 00005 *速度制御用ライブラリ 00006 * 00007 * サンプルプログラム 00008 * @code 00009 * //サンプルプログラム 差動二輪駆動ロボットを想定 00010 * //直進・後退・右旋回・左旋回が可能 00011 * #include "mbed.h" 00012 * #include "SpeedController.h" 00013 * 00014 * #define BASIC_SPEED 20 //モーターはこの角速度で駆動させる 00015 * 00016 * SpeedControl motorR(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //右輪 00017 * SpeedControl motorL(PF_1,PA_11,NC,1048,0.05,PA_9,PA_10); //左輪 00018 * Ticker motor_tick; //角速度計算用ticker 00019 * Serial pc(USBTX,USBRX); 00020 * 00021 * void calOmega() //角速度計算関数 00022 * { 00023 * motorR.CalOmega(); 00024 * motorL.CalOmega(); 00025 * } 00026 * 00027 * int main(void){ 00028 * motor_tick.attach(&calOmega,0.05); 00029 * //Cの値を設定 00030 * motorR.setDOconstant(46.3); 00031 * motorL.setDOconstant(47.2); 00032 * //pd係数を設定 00033 * motorR.setPDparam(0.1,0); 00034 * motorR.setPDparam(0.12,0); 00035 * 00036 * int kai=0; 00037 * double target_R=0,target_L=0; 00038 * char status[5][10]={"STOP","FORWARD","BACKWARD","RIGHT","LEFT"}; 00039 * int status_num=0; //初期状態は停止 00040 * 00041 * while(1) { 00042 * motorR.Sc(target_R); 00043 * motorL.Sc(target_L); //target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う 00044 * 00045 * if(kai>=500) { 00046 * pc.printf("status=%s omega_R=%f omega_L=%f \r\n",status[status_num],motorR.getOmega(),motorL.getOmega()); 00047 * //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 00048 * kai=0; 00049 * } 00050 * 00051 * if(pc.readable()) { 00052 * char sel=pc.getc(); 00053 * 00054 * switch (sel) { 00055 * case 's': //停止 00056 * motorR.stop(); 00057 * motorL.stop(); 00058 * target_R=0; 00059 * target_L=0; 00060 * status_num=0; 00061 * break; 00062 * case 'i': //前進 00063 * target_R=BASIC_SPEED; 00064 * target_L=BASIC_SPEED; 00065 * status_num=1; 00066 * break; 00067 * case 'm': //後退 00068 * target_R=(-1)*BASIC_SPEED; 00069 * target_L=(-1)*BASIC_SPEED; 00070 * status_num=2; 00071 * break; 00072 * case 'k': //右旋回 00073 * motorR.stop(); 00074 * target_R=0; 00075 * target_L=BASIC_SPEED; 00076 * status_num=3; 00077 * break; 00078 * case 'j': //左旋回 00079 * motorL.stop(); 00080 * target_R=BASIC_SPEED; 00081 * target_L=0; 00082 * status_num=4; 00083 * break; 00084 * default: 00085 * break; 00086 * } 00087 * if(sel=='q'){ 00088 * break; //qを押したら左右のモーターを停止 00089 * } 00090 * } 00091 * kai++; 00092 * } 00093 * motorR.stop(); 00094 * motorL.stop(); 00095 * 00096 * return 0; 00097 * } 00098 * @endcode 00099 * @section 基本的な速度制御の実用までの流れ 00100 * 00101 * 1. モーター・エンコーダ・モータードライバ・マイコン・バッテリーを適切につなげる 00102 * 00103 * 接続のイメージ↓ 00104 * 00105 * https://www.fastpic.jp/images.php?file=0391548523.png 00106 * 00107 * 注意!!! リポ用のスイッチと電圧計を必ずつけること!!! 00108 * 00109 * / 00110 * 00111 * 2. 実際にモーターを回転させてみて不備なく動作するか確認する 00112 * 00113 * 確認事項 00114 * 00115 * ・モーターが回るか、duty比によって加速・減速するか 00116 * 00117 * ・エンコーダの値が取れているか 00118 * 00119 * ・モーターの回転方向とエンコーダの加算減算が対応しているか (プログラム内で定義した正・負回転とエンコーダの増減が対応しているかということ) 00120 * 00121 * などなど。コードは下の一つ目のものを使えばいい 00122 * 00123 * / 00124 * 00125 * 3. Cの値を見つける 00126 * 00127 * このライブラリでは、「モーターにかかる電圧(duty比)と回転速度はある程度比例の関係にあるだろう」という考え方をしている(もちろん実際のモデルはもっと複雑である)。 00128 * 00129 * このライブラリにおける速度制御のアルゴリズムとしては、あらかじめ求めたこの比例係数を用いて、目標角速度からだいたいのduty比を算出し、そこからpd制御をかけるという形をとっている。 00130 * 00131 * Cとはこの比例係数のことである。一応式としてあらわすと、 00132 * 00133 * (回転速度)=C×(duty比) 00134 * 00135 * となる。このCの値はモーターやギアユニットによって固有の値であり、それぞれのモーターにおいて求める必要がある。 00136 * 00137 * 具体的なこの係数の求め方は、duty比を0.05くらいずつで変化させ、その時のduty比と回転角速度を記録し、excelからグラフの傾きを求めればよい。 00138 * 00139 * こんな感じになるよっていう画像↓ 00140 * 00141 * https://www.fastpic.jp/images.php?file=2416798781.gif 00142 * 00143 * コードは下の一つ目のものを使えばいい 00144 * 00145 * / 00146 * 00147 * 4. pd係数を見つける 00148 * 00149 * 上記の通り、最終的にはpd制御(精確にはi+pd制御)によって目標角速度に収束させる。 00150 * 00151 * ここではそのpd制御の操作量を決定する係数を見つける。これも実験的に求めるほかなく、値を変えて挙動を見てを繰り返して最適な係数を見つける。 00152 * 00153 * 値の評価は、目標に達するまでの速さ、オーバーシュートの少なさ、収束後の値の振動の小ささなどである。 00154 * 00155 * 重要度的にはp制御>d制御(場合によってはd制御が不必要なことも)なので、まずdの係数を0にした状態で先にpの係数を見つける。 00156 * 00157 * コードは下の二つ目のものを使えばいい 00158 * 00159 * @code 00160 * //2,3で使えるサンプルコード 00161 * //入力はTera Termで行う 00162 * #include "mbed.h" 00163 * #include "SpeedController.h" 00164 * 00165 * SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); 00166 * Serial pc(USBTX,USBRX); 00167 * Ticker motor_tick; 00168 * 00169 * void calOmega(){ 00170 * motor.CalOmega(); //角速度計算用 00171 * } 00172 * 00173 * int main(void){ 00174 * motor_tick.attach(calOmega,0.05); 00175 * int loop_time=0; 00176 * float in_duty=0; 00177 * bool print=false; 00178 * 00179 * while(1){ 00180 * loop_time++; 00181 * 00182 * if(in_duty>0) motor.turnF(in_duty); //in_dutyが正のとき正回転(このプログラムではPA_8で駆動) 00183 * else motor.turnB((-1)*in_duty); //in_dutyが負のとき逆回転(このプログラムではPB_5で駆動) 00184 * 00185 * 00186 * if(pc.readable()){ 00187 * char sel=pc.getc(); 00188 * if(sel=='s'){ 00189 * motor.stop(); 00190 * } else if(sel=='f'){ 00191 * in_duty+=(float)0.05; //fを押すとduty比がo.o5ずつあがる 00192 * pc.printf("duty=%f\r\n",in_duty); 00193 * } else if(sel=='b'){ 00194 * in_duty-=(float)0.05; //bを押すとduty比がo.o5ずつさがる 00195 * pc.printf("duty=%f\r\n",in_duty); 00196 * } else if(sel=='p'){ 00197 * print=!print; //pを押すと表示を停止/再開する 00198 * } 00199 * } 00200 * 00201 * if(loop_time%1000==0){ 00202 * 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_); 00203 * } //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示 00204 * } 00205 * } 00206 * @endcode 00207 * 00208 * @code 00209 * //PDの係数を探したいときに使えるサンプル 00210 * //入力はTera Termで行う 00211 * #include "mbed.h" 00212 * #include "SpeedController.h" 00213 * 00214 * #define TARGET_OMEGA 20 //目標角速度を20rad/sに設定 00215 * 00216 * SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定 00217 * Ticker motor_tick; //角速度計算用ticker 00218 * Serial pc(USBTX,USBRX); 00219 * 00220 * void calOmega() //角速度計算関数 00221 * { 00222 * motor.CalOmega(); 00223 * } 00224 * 00225 * int main() 00226 * { 00227 * motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 00228 * motor.setDOconstant(46.3); //求めたCの値を設定 00229 * 00230 * int kai=0; 00231 * double target_omega=0; 00232 * double P=0.0,D=0.0; 00233 * 00234 * while(1) { 00235 * motor.Sc(target_omega); //速度制御のコア関数 00236 * if(kai>=500) { 00237 * 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); 00238 * //エンコーダのカウント、角速度、目標角速度、正方向出力duty比、負方向出力duty比、Pの係数、Dの係数を表示 00239 * kai=0; 00240 * } 00241 * //係数探しの流れとしては、係数変更→速度制御開始→収束具合を観察・比較→モーター停止の繰り返し 00242 * if(pc.readable()) { 00243 * char sel=pc.getc(); 00244 * 00245 * switch (sel) { 00246 * case 'k': //kでpの係数に0.01加算 00247 * P+=0.01; 00248 * break; 00249 * case 'm': //mでpの係数に0.01減算 00250 * P-=0.01; 00251 * break; 00252 * case 'j': //jでdの係数に0.01加算 00253 * D+=0.01; 00254 * break; 00255 * case 'n': //nでdの係数に0.01減算 00256 * D-=0.01; 00257 * break; 00258 * case 's': 00259 * motor.stop(); //sでモーター停止 00260 * target_omega=0; 00261 * break; 00262 * case 'x': 00263 * target_omega=TARGET_OMEGA; //xで速度制御開始 00264 * break; 00265 * default: 00266 * break; 00267 * } 00268 * 00269 * if(sel=='q'){ 00270 * break; //qを押したら終了 00271 * } 00272 * 00273 * motor.setPDparam(P,D); //変更したパラメータをセット 00274 * } 00275 * kai++; 00276 * } 00277 * motor1.stop(); 00278 * } 00279 * @endcode 00280 * 00281 */ 00282 class SpeedControl : public Ec 00283 { 00284 private: 00285 double Kv_p,Kv_d; 00286 double diff,diff_old; 00287 double out_duty,out; 00288 double now_omega,now_RPM; 00289 double now_time_,old_time_; 00290 //Serial pc(USBTX,USBRX); 00291 protected: 00292 00293 public: 00294 PwmOut pwm_F_; 00295 PwmOut pwm_B_; 00296 00297 /** constractorの定義 00298 * @param signalA エンコーダA相のピン名 00299 * @param signalB エンコーダB相のピン名 00300 * @param signalZ エンコーダZ相のピン名 00301 * @param s エンコーダの分解能 00302 * @param t 角速度計算の間隔 00303 * @param pwm_F motorを正転させるpwmピン名 00304 * @param pwm_B motorを後転させるpwmピン名 00305 * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する 00306 */ 00307 SpeedControl( PinName signalA , PinName signalB , PinName signalZ , int s, double t, PinName pwm_F , PinName pwm_B); 00308 00309 /** duty比を角速度に変換させるための定数 00310 * 00311 * 使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める 00312 * 00313 * 多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない 00314 * 00315 * デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果 00316 * 00317 * 00318 */ 00319 double C; 00320 /** 速度制御関数、引数は目標速度 00321 * 00322 * この目標角加速度になるようにモーターを回転させることができる 00323 * 00324 * 負の数を代入すれば逆回転することができる 00325 * 00326 * 出力できるduty比は最大で0.5までに設定してある 00327 * @param target_omega 目標角速度 00328 * 00329 * @section CAUTION(printfについて) 00330 * 00331 * 上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。 00332 * 00333 * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。 00334 * 00335 * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。 00336 * 00337 * (どんなプログラムにも言えるが、)そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。 00338 * 00339 */ 00340 void Sc(double target_omega); 00341 /** PIDパラメータ設定関数 00342 * 00343 * 引数はそれぞれp,dパラメータ 00344 * 00345 * デフォルトは全部0に設定してある 00346 * 00347 * パラメーター値は実験的に頑張って求めるしかないです 00348 * @param p p制御パラメータ 00349 * @param d d制御パラメータ 00350 */ 00351 void setPDparam(double p,double d); 00352 00353 /** 角速度・duty比変換定数(C)の設定関数 00354 * 00355 * 文字通りである 00356 * @param c duty比を角速度に変換させるための定数 00357 */ 00358 void setDOconstant(double c); 00359 00360 /** モーター停止用関数 */ 00361 void stop(); 00362 00363 /** モーター正回転用関数 00364 @param duty 回転duty比 00365 */ 00366 void turnF(double duty); 00367 00368 /** モーター逆回転用関数 00369 @param duty 回転duty比 00370 */ 00371 void turnB(double duty); 00372 00373 /** 出力duty比 00374 */ 00375 double duty; 00376 00377 /** Z相を用いた速度制御 00378 @param target_RPM 目標RPM 00379 */ 00380 void ScZ2(double target_RPM); 00381 00382 //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ 00383 //void ScZ(double target_RPM); 00384 //void Accelarate(double target_duty); 00385 00386 virtual void reset(); 00387 }; 00388 #endif
Generated on Thu Aug 4 2022 11:33:19 by 1.7.2