2月25日
Dependencies: Encoder
Dependents: NHK2020-m2-2-ros_2_25
SpeedController.h@2:77256385cf03, 2019-08-09 (annotated)
- Committer:
- MazeTaka
- Date:
- Fri Aug 09 06:19:05 2019 +0000
- Revision:
- 2:77256385cf03
- Parent:
- 0:de44976b7670
- Child:
- 3:58a298386879
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| MazeTaka | 0:de44976b7670 | 1 | #ifndef _INCLUDE_SPEEDCONTROLLER_H_ |
| MazeTaka | 0:de44976b7670 | 2 | #define _INCLUDE_SPEEDCONTROLLER_H_ |
| MazeTaka | 0:de44976b7670 | 3 | |
| MazeTaka | 0:de44976b7670 | 4 | /** |
| MazeTaka | 0:de44976b7670 | 5 | * @brief モーターの速度制御用プログラム |
| MazeTaka | 0:de44976b7670 | 6 | * @details モーターのdutyと角速度がほぼ比例することを利用してPID制御を補正する |
| MazeTaka | 0:de44976b7670 | 7 | **/ |
| MazeTaka | 0:de44976b7670 | 8 | class SpeedControl |
| MazeTaka | 0:de44976b7670 | 9 | { |
| MazeTaka | 0:de44976b7670 | 10 | private: |
| MazeTaka | 0:de44976b7670 | 11 | double Kp_,Kd_; |
| MazeTaka | 0:de44976b7670 | 12 | double pre_diff; |
| MazeTaka | 0:de44976b7670 | 13 | double ptsc_; |
| MazeTaka | 0:de44976b7670 | 14 | double Cf_; |
| MazeTaka | 0:de44976b7670 | 15 | double Cb_; |
| MazeTaka | 0:de44976b7670 | 16 | /** 角速度(rad/s)をduty比に変換させるための定数 |
| MazeTaka | 0:de44976b7670 | 17 | * |
| MazeTaka | 0:de44976b7670 | 18 | * |
| MazeTaka | 0:de44976b7670 | 19 | * |
| MazeTaka | 0:de44976b7670 | 20 | **/ |
| MazeTaka | 0:de44976b7670 | 21 | double Df_; |
| MazeTaka | 0:de44976b7670 | 22 | double Db_; |
| MazeTaka | 0:de44976b7670 | 23 | double initial_Df_; |
| MazeTaka | 0:de44976b7670 | 24 | double initial_Db_; |
| MazeTaka | 0:de44976b7670 | 25 | double duty_limit_; |
| MazeTaka | 0:de44976b7670 | 26 | Ec *ec_; |
| MazeTaka | 0:de44976b7670 | 27 | |
| MazeTaka | 0:de44976b7670 | 28 | public: |
| MazeTaka | 0:de44976b7670 | 29 | PwmOut pwm_F_; |
| MazeTaka | 0:de44976b7670 | 30 | PwmOut pwm_B_; |
| MazeTaka | 0:de44976b7670 | 31 | |
| MazeTaka | 0:de44976b7670 | 32 | /** constractorの定義 |
| MazeTaka | 0:de44976b7670 | 33 | * @param pwm_F モーターを正転させるpwmピン名 |
| MazeTaka | 0:de44976b7670 | 34 | * @param pwm_B モーターを後転させるpwmピン名 |
| MazeTaka | 0:de44976b7670 | 35 | * @param us モーターのpwm周期[us] |
| MazeTaka | 0:de44976b7670 | 36 | * @param ec Encoder classのポインタ |
| MazeTaka | 0:de44976b7670 | 37 | * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する |
| MazeTaka | 0:de44976b7670 | 38 | */ |
| MazeTaka | 0:de44976b7670 | 39 | SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec); |
| MazeTaka | 0:de44976b7670 | 40 | |
| MazeTaka | 0:de44976b7670 | 41 | |
| MazeTaka | 0:de44976b7670 | 42 | /** 速度制御関数、引数は目標速度 |
| MazeTaka | 0:de44976b7670 | 43 | * |
| MazeTaka | 0:de44976b7670 | 44 | * この目標角加速度になるようにモーターを回転させることができる |
| MazeTaka | 0:de44976b7670 | 45 | * 負の数を代入すれば逆回転することができる |
| MazeTaka | 0:de44976b7670 | 46 | * |
| MazeTaka | 0:de44976b7670 | 47 | * @param target_omega 目標角速度 |
| MazeTaka | 0:de44976b7670 | 48 | * |
| MazeTaka | 0:de44976b7670 | 49 | * @section CAUTION（printfについて） |
| MazeTaka | 0:de44976b7670 | 50 | * |
| MazeTaka | 0:de44976b7670 | 51 | * 上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。 |
| MazeTaka | 0:de44976b7670 | 52 | * |
| MazeTaka | 0:de44976b7670 | 53 | * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。 |
| MazeTaka | 0:de44976b7670 | 54 | * |
| MazeTaka | 0:de44976b7670 | 55 | * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。 |
| MazeTaka | 0:de44976b7670 | 56 | * |
| MazeTaka | 0:de44976b7670 | 57 | * （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。 |
| MazeTaka | 0:de44976b7670 | 58 | * |
| MazeTaka | 0:de44976b7670 | 59 | */ |
| MazeTaka | 0:de44976b7670 | 60 | void Sc(double target_omega); |
| MazeTaka | 0:de44976b7670 | 61 | ///モーターのpwm周期の設定用関数[s] |
| MazeTaka | 0:de44976b7670 | 62 | void period(double s); |
| MazeTaka | 0:de44976b7670 | 63 | ///モーターのpwm周期の設定用関数[ms] |
| MazeTaka | 0:de44976b7670 | 64 | void period_ms(int ms); |
| MazeTaka | 0:de44976b7670 | 65 | ///モーターのpwm周期の設定用関数[us] |
| MazeTaka | 0:de44976b7670 | 66 | void period_us(int us); |
| MazeTaka | 0:de44976b7670 | 67 | /** PIDパラメータ設定関数 |
| MazeTaka | 0:de44976b7670 | 68 | * |
| MazeTaka | 0:de44976b7670 | 69 | * 引数はそれぞれp,dパラメータ |
| MazeTaka | 0:de44976b7670 | 70 | * |
| MazeTaka | 2:77256385cf03 | 71 | * パラメーター値は実験的に頑張って求めるしかない |
| MazeTaka | 0:de44976b7670 | 72 | * @param kp p制御パラメータ |
| MazeTaka | 0:de44976b7670 | 73 | * @param kd d制御パラメータ |
| MazeTaka | 0:de44976b7670 | 74 | */ |
| MazeTaka | 0:de44976b7670 | 75 | void setPDparam(double kp,double kd); |
| MazeTaka | 0:de44976b7670 | 76 | |
| MazeTaka | 0:de44976b7670 | 77 | /** 角速度・duty比の関係式の設定関数 |
| MazeTaka | 0:de44976b7670 | 78 | * 横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する |
| MazeTaka | 0:de44976b7670 | 79 | * @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時) |
| MazeTaka | 0:de44976b7670 | 80 | * @param df 角速度[rad/s]とduty比の関係式の切片(正転時) |
| MazeTaka | 0:de44976b7670 | 81 | * @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時) |
| MazeTaka | 0:de44976b7670 | 82 | * @param db 角速度[rad/s]とduty比の関係式の切片(逆転時) |
| MazeTaka | 0:de44976b7670 | 83 | */ |
| MazeTaka | 0:de44976b7670 | 84 | void setEquation(double cf,double df,double cb,double db); |
| MazeTaka | 2:77256385cf03 | 85 | /** 出力duty比の上限を決める関数 |
| MazeTaka | 2:77256385cf03 | 86 | * 0 ~ 0.95の範囲内で設定する |
| MazeTaka | 2:77256385cf03 | 87 | * @param duty_limit 出力duty比の上限 |
| MazeTaka | 2:77256385cf03 | 88 | */ |
| MazeTaka | 0:de44976b7670 | 89 | void setDutyLimit(double duty_limit); |
| MazeTaka | 0:de44976b7670 | 90 | |
| MazeTaka | 0:de44976b7670 | 91 | /** モーター停止用関数 |
| MazeTaka | 0:de44976b7670 | 92 | * dutyを0にする |
| MazeTaka | 0:de44976b7670 | 93 | */ |
| MazeTaka | 0:de44976b7670 | 94 | void stop(); |
| MazeTaka | 0:de44976b7670 | 95 | /** duty(-1 ~ 1)比でモーターを回転させる関数 |
| MazeTaka | 0:de44976b7670 | 96 | * 負の値を入れたら逆回転する |
| MazeTaka | 2:77256385cf03 | 97 | * @param duty 出力duty比 |
| MazeTaka | 0:de44976b7670 | 98 | */ |
| MazeTaka | 0:de44976b7670 | 99 | void turn(double duty); |
| MazeTaka | 0:de44976b7670 | 100 | ///Encoderや速度制御の値をresetする関数 |
| MazeTaka | 0:de44976b7670 | 101 | void reset(); |
| MazeTaka | 0:de44976b7670 | 102 | }; |
| MazeTaka | 0:de44976b7670 | 103 | |
| MazeTaka | 0:de44976b7670 | 104 | #endif |
| MazeTaka | 0:de44976b7670 | 105 |