File content as of revision 10:216d5a573dc7:

#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{
    private:
        int S;  //A相B相のパターン（1~4）
        bool stateA,stateB; //A・B相の状態
        int count;  //カウント数 分解能分で一周
        int pre_count;  //一つ前のカウント
        double precount;    //4倍精度カウント
        int solution;   //分解能
        double dt;  //角速度の計算間隔
        
        //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(!button) Ec1.reset();     //USERボタンを押したらリセット
         *      }
         *  }
         * @endcode
         */
        void CalOmega();
        ///omega(角速度 rad/s)の値を返す関数(double型)
        double getOmega();
        ///四倍精度のcountの値を返す関数(double型)
        double getPreCount();
        ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする
        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;
    
};

///速度制御用class 
///Ec classを継承している
/** @section Example
  * @code
  * #include "mbed.h"
  * #include "EC.h" //ヘッダファイルをインクルード
  *
  * SpeedControl motor1(PA_0,PA_1,1024,0.05,PB_8,PB_9); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定
  * Ticker ticker;  //角速度計算用ticker
  * DigitalIn button(USER_BUTTON);
  * Serial pc(USBTX,USBRX);
  *
  * void calOmega() //角速度計算関数
  * {
  *   motor1.CalOmega();
  * }
  *
  * int main()
  * {
  *    ticker.attach(&calOmega,0.05);      //0.05秒間隔で角速度を計算
  *    motor1.setPIDparam(0.15,0.0,0.15);  //PIDパラメータを設定
  *    motor1.setDOconstant(46.3);         //duty比と角速度の変換係数を46.3に設定
  *    
  *    int kai=0;
  *
  *    while(1) {
  *        motor1.Sc(-10);     //角速度10rad/sで逆回転
  *        if(kai>=500){       //ループ500回ごとに角速度・出力duty比を表示
  *            pc.printf("omega=%f duty=%f \r\n",motor1.getOmega(),motor1.duty);
  *            kai=0;
  *        }
  *        if(!button) break;  //ボタンを押したら停止
  *        kai++;
  *    }
  *    motor1.stop();
  * }
  * @endcode
  */


class SpeedControl : public Ec{
    private:
        double Kv_p,Kv_d,Kv_i;
        double diff,diff_old,integral;
        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,i,dパラメータ
         *
         *  デフォルトは全部0に設定してある
         *
         *  だいたいp=0.15くらいでなんとかなる（知らんけど）
         *  @param p p制御パラメータ
         *  @param i i制御パラメータ
         *  @param d d制御パラメータ
         */
         
        void setPIDparam(double p,double i,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;
        
        void ScZ(double target_RPM);
        void ScZ2(double target_RPM);
        void Accelarate(double target_duty);
};
#endif