Diff: EC.h

Revision:

0:20fc96400ca3

Child:

1:d10ca555a103

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EC.h	Thu Jun 16 07:36:15 2016 +0000
@@ -0,0 +1,219 @@
+#ifndef __INCLUDED_EC_H_
+#define __INCLUDED_EC_H_
+
+#ifndef M_pi
+#define M_pi 3.141592
+#endif
+/**increment型encoder用class*/
+
+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;  //角速度の計算間隔
+        
+        
+        InterruptIn signalA_;
+        InterruptIn signalB_;
+        
+        void upA();
+        void downA();
+        void upB();
+        void downB();
+    
+    public:
+        double omega;   //角速度
+        /** コンストラクタの定義
+         *
+         *  main関数の前に必ず一度宣言する
+         *
+         *  使うエンコーダの数だけ設定する必要がある
+         *
+         *  @param signalA エンコーダA相のピン名
+         *  @param signalB エンコーダB相のピン名
+         *  @param s エンコーダの分解能(省略可)
+         *  @param t 角速度計算の間隔(省略可)  
+         */
+        Ec(PinName signalA,PinName signalB,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,1024,0.05); //分解能1024、計算間隔0.05秒に設定
+         *  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);
+        
+        ///角速度計算の間隔
+        static double deftime;
+        ///エンコーダの分解能のデフォルト値
+        static int defsolution;
+        
+        
+    
+};
+
+///速度制御用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;
+        
+    protected:
+        PwmOut pwm_F_;
+        PwmOut pwm_B_;
+        
+    public:
+        /** constractorの定義
+         *  @param signalA encoderA相のピン名
+         *  @param signalB encoderB相のピン名
+         *  @param s encoderの分解能(省略可)
+         *  @param t 角速度計算の間隔(省略可)
+         *  @param pwm_F motorを正転させるpwmピン名
+         *  @param pwm_B motorを後転させるpwmピン名
+         *  motorを正転させるとencoderのcountが正のほうに加算されるようにencoderとmotorを設置する
+         */
+        SpeedControl( PinName signalA , PinName signalB , 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;
+};
+#endif
\ No newline at end of file