Diff: SpeedController.h

Revision:

0:de44976b7670

Child:

2:77256385cf03

diff -r 000000000000 -r de44976b7670 SpeedController.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SpeedController.h	Wed Jul 24 17:11:01 2019 +0000
@@ -0,0 +1,100 @@
+#ifndef _INCLUDE_SPEEDCONTROLLER_H_
+#define _INCLUDE_SPEEDCONTROLLER_H_
+
+/**
+* @brief モーターの速度制御用プログラム
+* @details モーターのdutyと角速度がほぼ比例することを利用してPID制御を補正する
+**/
+class SpeedControl
+{
+private:
+    double Kp_,Kd_;
+    double pre_diff;
+    double ptsc_;
+    double Cf_;
+    double Cb_;
+    /** 角速度(rad/s)をduty比に変換させるための定数
+     *
+     *
+     *
+     **/
+    double Df_;
+    double Db_;
+    double initial_Df_;
+    double initial_Db_;
+    double duty_limit_;
+    Ec *ec_;
+    
+public:
+    PwmOut pwm_F_;
+    PwmOut pwm_B_;
+
+    /** constractorの定義
+     *  @param pwm_F モーターを正転させるpwmピン名
+     *  @param pwm_B モーターを後転させるpwmピン名
+     *  @param us モーターのpwm周期[us]
+     *  @param ec Encoder classのポインタ
+     *  モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
+     */
+    SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec);
+
+
+    /** 速度制御関数、引数は目標速度
+     *
+     *  この目標角加速度になるようにモーターを回転させることができる
+     *  負の数を代入すれば逆回転することができる
+     *
+     *  @param target_omega 目標角速度
+     *
+     *  @section CAUTION（printfについて）
+     *
+     *  上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
+     *
+     *  printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
+     *
+     *  なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
+     *
+     *  （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
+     *
+     */
+    void Sc(double target_omega);
+    ///モーターのpwm周期の設定用関数[s]
+    void period(double s);
+    ///モーターのpwm周期の設定用関数[ms]
+    void period_ms(int ms);
+    ///モーターのpwm周期の設定用関数[us]
+    void period_us(int us);
+    /** PIDパラメータ設定関数
+     *
+     *  引数はそれぞれp,dパラメータ
+     *
+     *  パラメーター値は実験的に頑張って求めるしかないです
+     *  @param kp p制御パラメータ
+     *  @param kd d制御パラメータ
+     */
+    void setPDparam(double kp,double kd);
+
+    /** 角速度・duty比の関係式の設定関数
+     *  横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する
+     *  @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時)
+     *  @param df 角速度[rad/s]とduty比の関係式の切片(正転時)
+     *  @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時)
+     *  @param db 角速度[rad/s]とduty比の関係式の切片(逆転時)
+     */
+    void setEquation(double cf,double df,double cb,double db);
+    void setDutyLimit(double duty_limit);
+
+    /** モーター停止用関数
+    *   dutyを0にする
+    */
+    void stop();
+    /** duty(-1 ~ 1)比でモーターを回転させる関数
+    *   負の値を入れたら逆回転する
+    */
+    void turn(double duty);
+    ///Encoderや速度制御の値をresetする関数
+    void reset();
+};
+
+#endif
+