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