File content as of revision 1:163629043391:

#ifndef __INCLUDED_EC_H_
#define __INCLUDED_EC_H_

#ifndef M_pi
#define M_pi 3.141592
#endif
/**increment型encoder用class

   Z相(1周につき1回立ち上がる)の機能を追加しました!!!!
   
   普通に使う分には不必要な機能ですが、回転回数が欲しい場合や、回転が非常に早い物の回転速度が欲しい場合などでA,B相での処理だとマイコンが追いつかない場合などに使ってください
   */

class Ec{
    protected:
        int S;  //A相B相のパターン（1~4）
        bool stateA,stateB; //A・B相の状態
        int count;  //カウント数 分解能分で一周
        double pre_count;  //一つ前のカウント
        double precount;    //4倍精度カウント
        int solution;   //分解能
        double dt;  //角速度の計算間隔
        double C_ec;
        
        //z相用
        bool first;
        int rev;
        double now_time,old_time;
        double RPM,RPM_old;
        int RPM_th;
        
        InterruptIn signalA_;
        InterruptIn signalB_;
        InterruptIn signalZ_;
        
        void upA();
        void downA();
        void upB();
        void downB();
        void upZ();
        void downZ();
    
    public:
        double omega;   //角速度
        /** コンストラクタの定義
         *  
         *  ***Z相の機能を追加したことで引数が増えました!!!!***
         *
         *  main関数の前に必ず一度宣言する
         *
         *  使うエンコーダの数だけ設定する必要がある
         *
         *  @param signalA エンコーダA相のピン名
         *  @param signalB エンコーダB相のピン名
         *  @param signalZ エンコーダZ相のピン名
         *  @param s エンコーダの分解能(省略可)
         *  @param t 角速度計算の間隔(省略可)  
         */
        /** @section CAUTION
         *  今まで以下のように定義していたものは
         *  @code
         *  #include "mbed.h"
         *  #include "EC.h"
         *  
         *  Ec Ec1(PA_0,PA_1,1024,0.05);
         *  @endcode
         *  次のようにZ相の引数の部分に、NCと入れれば今までの様に使える
         *  @code
         *  #include "mbed.h"
         *  #include "EC.h"
         *  
         *  Ec Ec1(PA_0,PA_1,NC,1024,0.05);
         *  @endcode
         */
        Ec(PinName signalA,PinName signalB,PinName signalZ,int s,double t);
        
        ///countの値を返す関数(int型)
        int getCount();
        ///omega(角速度 rad/s)の値を計算する関数
        /** @section CAUTION
         *  CalOmega関数は、一定時間ごと（dtごと）に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。
         *  @code
         *  #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を押したらリセット
         *          }
         *      }
         *  }
         * @endcode
         */
        void CalOmega();
        ///omega(角速度 rad/s)の値を返す関数(double型)
        double getOmega();
        ///四倍精度のcountの値を返す関数(double型)
        double getPreCount();
        ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする
        //virtual void reset();
        ///角速度計算の間隔dtを決めることができる（デフォルトは0.05秒）
        void setTime(double t);
        ///(Z相を使用する場合)回転回数を返す関数(int型)
        int getRev();
        ///(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型)
        /** @section SAMPLE
         *  z相を使う場合のプログラムの例
         *  @code
         *  #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ボタンを押したらリセット
         *      }
         *  }
         * @endcode
         */
        double getRPM();
        
        void changeRPM_th(int th);

        
        ///角速度計算の間隔
        static double deftime;
        ///エンコーダの分解能のデフォルト値
        static int defsolution;
        
        Timer timer;
    
};


/**速度制御用ライブラリ
 *
 * サンプルプログラム
 * @code
 * //サンプルプログラム 差動二輪駆動ロボットを想定
 * //直進・後退・右旋回・左旋回が可能
 * #include "mbed.h"
 * #include "EC.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 "EC.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 "EC.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_plus,out_minus;
        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 Cf,Df,Cb,Db;
        /** 速度制御関数、引数は目標速度
         *
         *  この目標角加速度になるようにモーターを回転させることができる
         *
         *  負の数を代入すれば逆回転することができる
         *
         *  出力できる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 cf,double df,double cb,double db);
        
        /** モーター停止用関数 */
        void stop();
        
        /** モーター正回転用関数
            @param duty 回転duty比
        */
        void turnF(double duty);
        
        /** モーター逆回転用関数
            @param duty 回転duty比
        */
        void turnB(double duty);
        
        /** 出力duty比
        */
        double duty;
        double sth_debug;
        
        /** 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