riko-ten
ec
Fork of
EC
by 
ROBOSTEP_LIBRARY
Diff: EC.h
- Revision:
- 0:20fc96400ca3
- Child:
- 1:d10ca555a103
diff -r 000000000000 -r 20fc96400ca3 EC.h
--- /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