Diff: PID_SC.h

Revision:

36:ce11c30e303c

Parent:

32:297384f9d261

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_SC.h	Fri Jun 14 09:56:32 2019 +0000
@@ -0,0 +1,388 @@
+#ifndef INCLUDED_SPEEDCONTROLLER_SPEEDCONTROLLER_H
+#define INCLUDED_SPEEDCONTROLLER_SPEEDCONTROLLER_H
+#include "EC.h"
+/** @class SpeedControl
+*速度制御用ライブラリ
+ *
+ * サンプルプログラム
+ * @code
+ * //サンプルプログラム 差動二輪駆動ロボットを想定
+ * //直進・後退・右旋回・左旋回が可能
+ * #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;
+ * }
+ * @endcode
+ * @section 基本的な速度制御の実用までの流れ
+ *
+ * 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の係数を見つける。
+ *
+ * コードは下の二つ目のものを使えばいい
+ *
+ * @code
+ * //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比を表示
+ *     }
+ * }
+ * @endcode
+ *
+ * @code
+ * //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();
+ * }
+ * @endcode
+ *
+ */
+class SpeedControl : public Ec
+{
+private:
+    double Kv_p,Kv_d;
+    double diff,diff_old;
+    double out_duty,out;
+    double now_omega,now_RPM;
+    double now_time_,old_time_;
+    //Serial pc(USBTX,USBRX);
+protected:
+
+public:
+    PwmOut pwm_F_;
+    PwmOut pwm_B_;
+
+    /** constractorの定義
+     *  @param signalA エンコーダA相のピン名
+     *  @param signalB エンコーダB相のピン名
+     *  @param signalZ エンコーダZ相のピン名
+     *  @param s エンコーダの分解能
+     *  @param t 角速度計算の間隔
+     *  @param pwm_F motorを正転させるpwmピン名
+     *  @param pwm_B motorを後転させるpwmピン名
+     *  モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
+     */
+    SpeedControl( PinName signalA , PinName signalB , PinName signalZ , int s, double t, PinName pwm_F , PinName pwm_B);
+
+    /** duty比を角速度に変換させるための定数
+     *
+     *  使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める
+     *
+     *  多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない
+     *
+     *  デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果
+     *
+     *
+     */
+    double C;
+    /** 速度制御関数、引数は目標速度
+     *
+     *  この目標角加速度になるようにモーターを回転させることができる
+     *
+     *  負の数を代入すれば逆回転することができる
+     *
+     *  出力できるduty比は最大で0.5までに設定してある
+     *  @param target_omega 目標角速度
+     *
+     *  @section CAUTION（printfについて）
+     *
+     *  上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
+     *
+     *  printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
+     *
+     *  なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
+     *
+     *  （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
+     *
+     */
+    void Sc(double target_omega);
+    /** PIDパラメータ設定関数
+     *
+     *  引数はそれぞれp,dパラメータ
+     *
+     *  デフォルトは全部0に設定してある
+     *
+     *  パラメーター値は実験的に頑張って求めるしかないです
+     *  @param p p制御パラメータ
+     *  @param d d制御パラメータ
+     */
+    void setPDparam(double p,double d);
+
+    /** 角速度・duty比変換定数(C)の設定関数
+     *
+     *  文字通りである
+     *  @param c duty比を角速度に変換させるための定数
+     */
+    void setDOconstant(double c);
+
+    /** モーター停止用関数 */
+    void stop();
+
+    /** モーター正回転用関数
+        @param duty 回転duty比
+    */
+    void turnF(double duty);
+
+    /** モーター逆回転用関数
+        @param duty 回転duty比
+    */
+    void turnB(double duty);
+
+    /** 出力duty比
+    */
+    double duty;
+
+    /** Z相を用いた速度制御
+        @param target_RPM 目標RPM
+    */
+    void ScZ2(double target_RPM);
+
+    //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
+    //void ScZ(double target_RPM);
+    //void Accelarate(double target_duty);
+
+    virtual void reset();
+};
+#endif
\ No newline at end of file