ロボステ6期
/
SpeedController2_25
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder
SpeedController.h@6:d110c276aa5a, 2019-08-10 (annotated)
- Committer:
- MazeTaka
- Date:
- Sat Aug 10 05:20:49 2019 +0000
- Revision:
- 6:d110c276aa5a
- Parent:
- 5:fddbe92ca86f
- Child:
- 7:b1708b8819f1
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 | 5:fddbe92ca86f | 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 | double Df_; |
| MazeTaka | 0:de44976b7670 | 17 | double Db_; |
| MazeTaka | 0:de44976b7670 | 18 | double initial_Df_; |
| MazeTaka | 0:de44976b7670 | 19 | double initial_Db_; |
| MazeTaka | 0:de44976b7670 | 20 | double duty_limit_; |
| MazeTaka | 0:de44976b7670 | 21 | Ec *ec_; |
| MazeTaka | 4:ee0ac8415639 | 22 | |
| MazeTaka | 0:de44976b7670 | 23 | public: |
| MazeTaka | 0:de44976b7670 | 24 | PwmOut pwm_F_; |
| MazeTaka | 0:de44976b7670 | 25 | PwmOut pwm_B_; |
| MazeTaka | 0:de44976b7670 | 26 | |
| MazeTaka | 0:de44976b7670 | 27 | /** constractorの定義 |
| MazeTaka | 6:d110c276aa5a | 28 | * @param pwm_F モーターを正転させるpwmピン名 |
| MazeTaka | 6:d110c276aa5a | 29 | * @param pwm_B モーターを後転させるpwmピン名 |
| MazeTaka | 6:d110c276aa5a | 30 | * @param us モーターのpwm周期[us] |
| MazeTaka | 6:d110c276aa5a | 31 | * @param ec Encoder classのポインタ |
| MazeTaka | 6:d110c276aa5a | 32 | * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する |
| MazeTaka | 6:d110c276aa5a | 33 | */ |
| MazeTaka | 0:de44976b7670 | 34 | SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec); |
| MazeTaka | 0:de44976b7670 | 35 | |
| MazeTaka | 0:de44976b7670 | 36 | |
| MazeTaka | 5:fddbe92ca86f | 37 | |
| MazeTaka | 5:fddbe92ca86f | 38 | /**モーターのpwm周期の設定用関数 |
| MazeTaka | 5:fddbe92ca86f | 39 | * @param s モーターのpwm周期[s] |
| MazeTaka | 5:fddbe92ca86f | 40 | */ |
| MazeTaka | 5:fddbe92ca86f | 41 | void period(double s); |
| MazeTaka | 5:fddbe92ca86f | 42 | /**モーターのpwm周期の設定用関数 |
| MazeTaka | 5:fddbe92ca86f | 43 | * @param ms モーターのpwm周期[ms] |
| MazeTaka | 5:fddbe92ca86f | 44 | */ |
| MazeTaka | 5:fddbe92ca86f | 45 | void period_ms(int ms); |
| MazeTaka | 5:fddbe92ca86f | 46 | /**モーターのpwm周期の設定用関数 |
| MazeTaka | 5:fddbe92ca86f | 47 | * @param us モーターのpwm周期[us] |
| MazeTaka | 6:d110c276aa5a | 48 | * |
| MazeTaka | 6:d110c276aa5a | 49 | * @senction SAMPLE |
| MazeTaka | 6:d110c276aa5a | 50 | * @code |
| MazeTaka | 6:d110c276aa5a | 51 | * SpeedControl motor(p21,p22,50,EC); |
| MazeTaka | 6:d110c276aa5a | 52 | * int main(){ |
| MazeTaka | 6:d110c276aa5a | 53 | * motor.period_us(50); |
| MazeTaka | 6:d110c276aa5a | 54 | * while(1){ |
| MazeTaka | 6:d110c276aa5a | 55 | * @endcode |
| MazeTaka | 5:fddbe92ca86f | 56 | */ |
| MazeTaka | 5:fddbe92ca86f | 57 | void period_us(int us); |
| MazeTaka | 5:fddbe92ca86f | 58 | /** PIDパラメータ設定関数 |
| MazeTaka | 6:d110c276aa5a | 59 | * |
| MazeTaka | 6:d110c276aa5a | 60 | * 引数はそれぞれp,dパラメータ |
| MazeTaka | 6:d110c276aa5a | 61 | * |
| MazeTaka | 6:d110c276aa5a | 62 | * パラメーター値は実験的に頑張って求めるしかない |
| MazeTaka | 6:d110c276aa5a | 63 | * @param kp p制御パラメータ |
| MazeTaka | 6:d110c276aa5a | 64 | * @param kd d制御パラメータ |
| MazeTaka | 6:d110c276aa5a | 65 | * |
| MazeTaka | 6:d110c276aa5a | 66 | * @senction SAMPLE |
| MazeTaka | 6:d110c276aa5a | 67 | * @code |
| MazeTaka | 6:d110c276aa5a | 68 | * SpeedControl motor(p21,p22,50,EC); |
| MazeTaka | 6:d110c276aa5a | 69 | * int main(){ |
| MazeTaka | 6:d110c276aa5a | 70 | * motor.setPDparam(0.1,0.001); |
| MazeTaka | 6:d110c276aa5a | 71 | * while(1){ |
| MazeTaka | 6:d110c276aa5a | 72 | * @endcode |
| MazeTaka | 6:d110c276aa5a | 73 | */ |
| MazeTaka | 5:fddbe92ca86f | 74 | void setPDparam(double kp,double kd); |
| MazeTaka | 5:fddbe92ca86f | 75 | |
| MazeTaka | 5:fddbe92ca86f | 76 | /** 角速度・duty比の関係式の設定関数 |
| MazeTaka | 6:d110c276aa5a | 77 | * 横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する |
| MazeTaka | 6:d110c276aa5a | 78 | * @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時) |
| MazeTaka | 6:d110c276aa5a | 79 | * @param df 角速度[rad/s]とduty比の関係式の切片(正転時) |
| MazeTaka | 6:d110c276aa5a | 80 | * @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時) |
| MazeTaka | 6:d110c276aa5a | 81 | * @param db 角速度[rad/s]とduty比の関係式の切片(逆転時) |
| MazeTaka | 6:d110c276aa5a | 82 | * |
| MazeTaka | 6:d110c276aa5a | 83 | * @senction SAMPLE |
| MazeTaka | 6:d110c276aa5a | 84 | * @code |
| MazeTaka | 6:d110c276aa5a | 85 | * SpeedControl motor(p21,p22,50,EC); |
| MazeTaka | 6:d110c276aa5a | 86 | * int main(){ |
| MazeTaka | 6:d110c276aa5a | 87 | * motor.setEquation(0.02,0.0001,-0.02,0.0001); |
| MazeTaka | 6:d110c276aa5a | 88 | * while(1){ |
| MazeTaka | 6:d110c276aa5a | 89 | * @endcode |
| MazeTaka | 6:d110c276aa5a | 90 | */ |
| MazeTaka | 5:fddbe92ca86f | 91 | void setEquation(double cf,double df,double cb,double db); |
| MazeTaka | 5:fddbe92ca86f | 92 | /** 出力duty比の上限を決める関数 |
| MazeTaka | 5:fddbe92ca86f | 93 | * 0 ~ 0.95の範囲内で設定する |
| MazeTaka | 5:fddbe92ca86f | 94 | * @param duty_limit 出力duty比の上限 |
| MazeTaka | 6:d110c276aa5a | 95 | * |
| MazeTaka | 6:d110c276aa5a | 96 | * @senction SAMPLE |
| MazeTaka | 6:d110c276aa5a | 97 | * @code |
| MazeTaka | 6:d110c276aa5a | 98 | * SpeedControl motor(p21,p22,50,EC); |
| MazeTaka | 6:d110c276aa5a | 99 | * int main(){ |
| MazeTaka | 6:d110c276aa5a | 100 | * motor.setDutyLimit(0.5); //duty比の上限を0.5に設定 |
| MazeTaka | 6:d110c276aa5a | 101 | * while(1){ |
| MazeTaka | 6:d110c276aa5a | 102 | * @endcode |
| MazeTaka | 5:fddbe92ca86f | 103 | */ |
| MazeTaka | 5:fddbe92ca86f | 104 | void setDutyLimit(double duty_limit); |
| MazeTaka | 0:de44976b7670 | 105 | /** 速度制御関数、引数は目標速度 |
| MazeTaka | 6:d110c276aa5a | 106 | * |
| MazeTaka | 6:d110c276aa5a | 107 | * この目標角加速度になるようにモーターを回転させることができる |
| MazeTaka | 6:d110c276aa5a | 108 | * 負の数を代入すれば逆回転することができる |
| MazeTaka | 6:d110c276aa5a | 109 | * |
| MazeTaka | 6:d110c276aa5a | 110 | * @param target_omega 目標角速度 |
| MazeTaka | 6:d110c276aa5a | 111 | * |
| MazeTaka | 6:d110c276aa5a | 112 | * @section SAMPLE |
| MazeTaka | 6:d110c276aa5a | 113 | * @code |
| MazeTaka | 6:d110c276aa5a | 114 | * //プログラム例 |
| MazeTaka | 6:d110c276aa5a | 115 | * #include "mbed.h" |
| MazeTaka | 6:d110c276aa5a | 116 | * #include "EC.h" //Encoderライブラリをインクルード |
| MazeTaka | 6:d110c276aa5a | 117 | * #include "SpeedController.h" //SpeedControlライブラリをインクルード |
| MazeTaka | 6:d110c276aa5a | 118 | * #define RESOLUTION 500 |
| MazeTaka | 6:d110c276aa5a | 119 | * Ec4multi EC(p11,p12,RESOLUTION); |
| MazeTaka | 6:d110c276aa5a | 120 | * SpeedControl motor(p21,p22,50,EC); |
| MazeTaka | 6:d110c276aa5a | 121 | * int main(){ |
| MazeTaka | 6:d110c276aa5a | 122 | * motor.setEquation(0.0062,0.0045,-0.0061,0.0091); //求めたC,Dの値を設定 |
| MazeTaka | 6:d110c276aa5a | 123 | * motor.setPDparam(0.1,0.0); //PIDの係数を設定 |
| MazeTaka | 6:d110c276aa5a | 124 | * int kai=0; |
| MazeTaka | 6:d110c276aa5a | 125 | * for(int i=0;i<100000000;i++){ |
| MazeTaka | 6:d110c276aa5a | 126 | * motor.Sc(10); //10[rad/s]でモーターを回転 |
| MazeTaka | 6:d110c276aa5a | 127 | * kai++; |
| MazeTaka | 6:d110c276aa5a | 128 | * if(kai>1000){ |
| MazeTaka | 6:d110c276aa5a | 129 | * printf("omega=%f\r\n",EC.getOmega()); |
| MazeTaka | 6:d110c276aa5a | 130 | * kai=0; |
| MazeTaka | 6:d110c276aa5a | 131 | * } |
| MazeTaka | 6:d110c276aa5a | 132 | * } |
| MazeTaka | 6:d110c276aa5a | 133 | * motor.stop() |
| MazeTaka | 6:d110c276aa5a | 134 | * while(1); |
| MazeTaka | 6:d110c276aa5a | 135 | * } |
| MazeTaka | 6:d110c276aa5a | 136 | * @endcode |
| MazeTaka | 6:d110c276aa5a | 137 | * |
| MazeTaka | 6:d110c276aa5a | 138 | * @section CAUTION（printfの負荷について） |
| MazeTaka | 6:d110c276aa5a | 139 | * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。 |
| MazeTaka | 6:d110c276aa5a | 140 | * |
| MazeTaka | 6:d110c276aa5a | 141 | * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。 |
| MazeTaka | 6:d110c276aa5a | 142 | * |
| MazeTaka | 6:d110c276aa5a | 143 | * （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。 |
| MazeTaka | 6:d110c276aa5a | 144 | */ |
| MazeTaka | 0:de44976b7670 | 145 | void Sc(double target_omega); |
| MazeTaka | 0:de44976b7670 | 146 | /** モーター停止用関数 |
| MazeTaka | 0:de44976b7670 | 147 | * dutyを0にする |
| MazeTaka | 0:de44976b7670 | 148 | */ |
| MazeTaka | 0:de44976b7670 | 149 | void stop(); |
| MazeTaka | 0:de44976b7670 | 150 | /** duty(-1 ~ 1)比でモーターを回転させる関数 |
| MazeTaka | 0:de44976b7670 | 151 | * 負の値を入れたら逆回転する |
| MazeTaka | 2:77256385cf03 | 152 | * @param duty 出力duty比 |
| MazeTaka | 4:ee0ac8415639 | 153 | * @section SAMPLE |
| MazeTaka | 4:ee0ac8415639 | 154 | * @code |
| MazeTaka | 4:ee0ac8415639 | 155 | * #include "mbed.h" |
| MazeTaka | 4:ee0ac8415639 | 156 | * #include "EC.h" //Encoderライブラリをインクルード |
| MazeTaka | 4:ee0ac8415639 | 157 | * #include "SpeedController.h" //SpeedControlライブラリをインクルード |
| MazeTaka | 4:ee0ac8415639 | 158 | * #define RESOLUTION 500 |
| MazeTaka | 4:ee0ac8415639 | 159 | * Ec4multi EC(p11,p12,RESOLUTION); |
| MazeTaka | 4:ee0ac8415639 | 160 | * SpeedControl motor(p21,p22,50,EC); |
| MazeTaka | 4:ee0ac8415639 | 161 | * int main(){ |
| MazeTaka | 4:ee0ac8415639 | 162 | * while(1){ |
| MazeTaka | 5:fddbe92ca86f | 163 | * motor.turn(0.5); //duty0.5で正転 |
| MazeTaka | 4:ee0ac8415639 | 164 | * wait(1); |
| MazeTaka | 5:fddbe92ca86f | 165 | * motor.turn(0); //停止 |
| MazeTaka | 4:ee0ac8415639 | 166 | * wait(1); |
| MazeTaka | 5:fddbe92ca86f | 167 | * motor.turn(-0.5); //duty0.5で逆転 |
| MazeTaka | 4:ee0ac8415639 | 168 | * wait(1); |
| MazeTaka | 5:fddbe92ca86f | 169 | * motor.turn(0); //停止 |
| MazeTaka | 4:ee0ac8415639 | 170 | * wait(1); |
| MazeTaka | 4:ee0ac8415639 | 171 | * } |
| MazeTaka | 4:ee0ac8415639 | 172 | * } |
| MazeTaka | 4:ee0ac8415639 | 173 | * @endcode |
| MazeTaka | 0:de44976b7670 | 174 | */ |
| MazeTaka | 0:de44976b7670 | 175 | void turn(double duty); |
| MazeTaka | 0:de44976b7670 | 176 | ///Encoderや速度制御の値をresetする関数 |
| MazeTaka | 0:de44976b7670 | 177 | void reset(); |
| MazeTaka | 0:de44976b7670 | 178 | }; |
| MazeTaka | 0:de44976b7670 | 179 | |
| MazeTaka | 6:d110c276aa5a | 180 | #endif |