ロボステ6期
/
ball_snatch
e
SpeedController/SpeedController.h@0:a40701dd7f26, 2019-12-13 (annotated)
- Committer:
- aoikoizumi
- Date:
- Fri Dec 13 06:49:35 2019 +0000
- Revision:
- 0:a40701dd7f26
e
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aoikoizumi | 0:a40701dd7f26 | 1 | #ifndef _INCLUDE_SPEEDCONTROLLER_H_ |
aoikoizumi | 0:a40701dd7f26 | 2 | #define _INCLUDE_SPEEDCONTROLLER_H_ |
aoikoizumi | 0:a40701dd7f26 | 3 | |
aoikoizumi | 0:a40701dd7f26 | 4 | /** |
aoikoizumi | 0:a40701dd7f26 | 5 | * @brief モーターの速度制御用プログラム |
aoikoizumi | 0:a40701dd7f26 | 6 | * @details モーターのdutyと角速度がほぼ比例することを利用して出力を決定し、PID制御で補正する |
aoikoizumi | 0:a40701dd7f26 | 7 | **/ |
aoikoizumi | 0:a40701dd7f26 | 8 | class SpeedControl |
aoikoizumi | 0:a40701dd7f26 | 9 | { |
aoikoizumi | 0:a40701dd7f26 | 10 | private: |
aoikoizumi | 0:a40701dd7f26 | 11 | double Kp_,Kd_; |
aoikoizumi | 0:a40701dd7f26 | 12 | double pre_diff; |
aoikoizumi | 0:a40701dd7f26 | 13 | double ptsc_; |
aoikoizumi | 0:a40701dd7f26 | 14 | double Cf_; |
aoikoizumi | 0:a40701dd7f26 | 15 | double Cb_; |
aoikoizumi | 0:a40701dd7f26 | 16 | double Df_; |
aoikoizumi | 0:a40701dd7f26 | 17 | double Db_; |
aoikoizumi | 0:a40701dd7f26 | 18 | double initial_Df_; |
aoikoizumi | 0:a40701dd7f26 | 19 | double initial_Db_; |
aoikoizumi | 0:a40701dd7f26 | 20 | double duty_limit_; |
aoikoizumi | 0:a40701dd7f26 | 21 | Ec *ec_; |
aoikoizumi | 0:a40701dd7f26 | 22 | |
aoikoizumi | 0:a40701dd7f26 | 23 | public: |
aoikoizumi | 0:a40701dd7f26 | 24 | PwmOut pwm_F_; |
aoikoizumi | 0:a40701dd7f26 | 25 | PwmOut pwm_B_; |
aoikoizumi | 0:a40701dd7f26 | 26 | |
aoikoizumi | 0:a40701dd7f26 | 27 | /** constractorの定義 |
aoikoizumi | 0:a40701dd7f26 | 28 | * @param pwm_F モーターを正転させるpwmピン名 |
aoikoizumi | 0:a40701dd7f26 | 29 | * @param pwm_B モーターを後転させるpwmピン名 |
aoikoizumi | 0:a40701dd7f26 | 30 | * @param us モーターのpwm周期[us] |
aoikoizumi | 0:a40701dd7f26 | 31 | * @param ec Encoder classのポインタ |
aoikoizumi | 0:a40701dd7f26 | 32 | * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する |
aoikoizumi | 0:a40701dd7f26 | 33 | */ |
aoikoizumi | 0:a40701dd7f26 | 34 | SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec); |
aoikoizumi | 0:a40701dd7f26 | 35 | |
aoikoizumi | 0:a40701dd7f26 | 36 | |
aoikoizumi | 0:a40701dd7f26 | 37 | |
aoikoizumi | 0:a40701dd7f26 | 38 | /**モーターのpwm周期の設定用関数 |
aoikoizumi | 0:a40701dd7f26 | 39 | * @param s モーターのpwm周期[s] |
aoikoizumi | 0:a40701dd7f26 | 40 | */ |
aoikoizumi | 0:a40701dd7f26 | 41 | void period(double s); |
aoikoizumi | 0:a40701dd7f26 | 42 | /**モーターのpwm周期の設定用関数 |
aoikoizumi | 0:a40701dd7f26 | 43 | * @param ms モーターのpwm周期[ms] |
aoikoizumi | 0:a40701dd7f26 | 44 | */ |
aoikoizumi | 0:a40701dd7f26 | 45 | void period_ms(int ms); |
aoikoizumi | 0:a40701dd7f26 | 46 | /**モーターのpwm周期の設定用関数 |
aoikoizumi | 0:a40701dd7f26 | 47 | * @param us モーターのpwm周期[us] |
aoikoizumi | 0:a40701dd7f26 | 48 | * |
aoikoizumi | 0:a40701dd7f26 | 49 | * @senction SAMPLE |
aoikoizumi | 0:a40701dd7f26 | 50 | * @code |
aoikoizumi | 0:a40701dd7f26 | 51 | * Ec4multi EC(p11,p12,500); |
aoikoizumi | 0:a40701dd7f26 | 52 | * SpeedControl motor(p21,p22,50,EC); |
aoikoizumi | 0:a40701dd7f26 | 53 | * int main(){ |
aoikoizumi | 0:a40701dd7f26 | 54 | * motor.period_us(50); |
aoikoizumi | 0:a40701dd7f26 | 55 | * while(1){ |
aoikoizumi | 0:a40701dd7f26 | 56 | * @endcode |
aoikoizumi | 0:a40701dd7f26 | 57 | */ |
aoikoizumi | 0:a40701dd7f26 | 58 | void period_us(int us); |
aoikoizumi | 0:a40701dd7f26 | 59 | /** PIDパラメータ設定関数 |
aoikoizumi | 0:a40701dd7f26 | 60 | * |
aoikoizumi | 0:a40701dd7f26 | 61 | * 引数はそれぞれp,dパラメータ |
aoikoizumi | 0:a40701dd7f26 | 62 | * |
aoikoizumi | 0:a40701dd7f26 | 63 | * パラメーター値は実験的に頑張って求めるしかない |
aoikoizumi | 0:a40701dd7f26 | 64 | * @param kp p制御パラメータ |
aoikoizumi | 0:a40701dd7f26 | 65 | * @param kd d制御パラメータ |
aoikoizumi | 0:a40701dd7f26 | 66 | * |
aoikoizumi | 0:a40701dd7f26 | 67 | * @senction SAMPLE |
aoikoizumi | 0:a40701dd7f26 | 68 | * @code |
aoikoizumi | 0:a40701dd7f26 | 69 | * Ec4multi EC(p11,p12,500); |
aoikoizumi | 0:a40701dd7f26 | 70 | * SpeedControl motor(p21,p22,50,EC); |
aoikoizumi | 0:a40701dd7f26 | 71 | * int main(){ |
aoikoizumi | 0:a40701dd7f26 | 72 | * motor.setPDparam(0.1,0.001); |
aoikoizumi | 0:a40701dd7f26 | 73 | * while(1){ |
aoikoizumi | 0:a40701dd7f26 | 74 | * @endcode |
aoikoizumi | 0:a40701dd7f26 | 75 | */ |
aoikoizumi | 0:a40701dd7f26 | 76 | void setPDparam(double kp,double kd); |
aoikoizumi | 0:a40701dd7f26 | 77 | |
aoikoizumi | 0:a40701dd7f26 | 78 | /** 角速度・duty比の関係式の設定関数 |
aoikoizumi | 0:a40701dd7f26 | 79 | * 横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する |
aoikoizumi | 0:a40701dd7f26 | 80 | * @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時) |
aoikoizumi | 0:a40701dd7f26 | 81 | * @param df 角速度[rad/s]とduty比の関係式の切片(正転時) |
aoikoizumi | 0:a40701dd7f26 | 82 | * @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時) |
aoikoizumi | 0:a40701dd7f26 | 83 | * @param db 角速度[rad/s]とduty比の関係式の切片(逆転時) |
aoikoizumi | 0:a40701dd7f26 | 84 | * |
aoikoizumi | 0:a40701dd7f26 | 85 | * @senction SAMPLE |
aoikoizumi | 0:a40701dd7f26 | 86 | * @code |
aoikoizumi | 0:a40701dd7f26 | 87 | * Ec4multi EC(p11,p12,500); |
aoikoizumi | 0:a40701dd7f26 | 88 | * SpeedControl motor(p21,p22,50,EC); |
aoikoizumi | 0:a40701dd7f26 | 89 | * int main(){ |
aoikoizumi | 0:a40701dd7f26 | 90 | * motor.setEquation(0.02,0.0001,-0.02,0.0001); |
aoikoizumi | 0:a40701dd7f26 | 91 | * while(1){ |
aoikoizumi | 0:a40701dd7f26 | 92 | * @endcode |
aoikoizumi | 0:a40701dd7f26 | 93 | */ |
aoikoizumi | 0:a40701dd7f26 | 94 | void setEquation(double cf,double df,double cb,double db); |
aoikoizumi | 0:a40701dd7f26 | 95 | /** 出力duty比の上限を決める関数 |
aoikoizumi | 0:a40701dd7f26 | 96 | * 0 ~ 0.95の範囲内で設定する |
aoikoizumi | 0:a40701dd7f26 | 97 | * @param duty_limit 出力duty比の上限 |
aoikoizumi | 0:a40701dd7f26 | 98 | * |
aoikoizumi | 0:a40701dd7f26 | 99 | * @senction SAMPLE |
aoikoizumi | 0:a40701dd7f26 | 100 | * @code |
aoikoizumi | 0:a40701dd7f26 | 101 | * Ec4multi EC(p11,p12,500); |
aoikoizumi | 0:a40701dd7f26 | 102 | * SpeedControl motor(p21,p22,50,EC); |
aoikoizumi | 0:a40701dd7f26 | 103 | * int main(){ |
aoikoizumi | 0:a40701dd7f26 | 104 | * motor.setDutyLimit(0.5); //duty比の上限を0.5に設定 |
aoikoizumi | 0:a40701dd7f26 | 105 | * while(1){ |
aoikoizumi | 0:a40701dd7f26 | 106 | * @endcode |
aoikoizumi | 0:a40701dd7f26 | 107 | */ |
aoikoizumi | 0:a40701dd7f26 | 108 | void setDutyLimit(double duty_limit); |
aoikoizumi | 0:a40701dd7f26 | 109 | /** 速度制御関数、引数は目標速度 |
aoikoizumi | 0:a40701dd7f26 | 110 | * |
aoikoizumi | 0:a40701dd7f26 | 111 | * この目標角加速度になるようにモーターを回転させることができる |
aoikoizumi | 0:a40701dd7f26 | 112 | * 負の数を代入すれば逆回転することができる |
aoikoizumi | 0:a40701dd7f26 | 113 | * |
aoikoizumi | 0:a40701dd7f26 | 114 | * @param target_omega 目標角速度 |
aoikoizumi | 0:a40701dd7f26 | 115 | * |
aoikoizumi | 0:a40701dd7f26 | 116 | * @section SAMPLE |
aoikoizumi | 0:a40701dd7f26 | 117 | * @code |
aoikoizumi | 0:a40701dd7f26 | 118 | * //プログラム例 |
aoikoizumi | 0:a40701dd7f26 | 119 | * #include "mbed.h" |
aoikoizumi | 0:a40701dd7f26 | 120 | * #include "EC.h" //Encoderライブラリをインクルード |
aoikoizumi | 0:a40701dd7f26 | 121 | * #include "SpeedController.h" //SpeedControlライブラリをインクルード |
aoikoizumi | 0:a40701dd7f26 | 122 | * #define RESOLUTION 500 |
aoikoizumi | 0:a40701dd7f26 | 123 | * Ec4multi EC(p11,p12,RESOLUTION); |
aoikoizumi | 0:a40701dd7f26 | 124 | * SpeedControl motor(p21,p22,50,EC); |
aoikoizumi | 0:a40701dd7f26 | 125 | * int main(){ |
aoikoizumi | 0:a40701dd7f26 | 126 | * motor.setEquation(0.0062,0.0045,-0.0061,0.0091); //求めたC,Dの値を設定 |
aoikoizumi | 0:a40701dd7f26 | 127 | * motor.setPDparam(0.1,0.0); //PIDの係数を設定 |
aoikoizumi | 0:a40701dd7f26 | 128 | * int kai=0; |
aoikoizumi | 0:a40701dd7f26 | 129 | * for(int i=0;i<100000000;i++){ |
aoikoizumi | 0:a40701dd7f26 | 130 | * motor.Sc(10); //10[rad/s]でモーターを回転 |
aoikoizumi | 0:a40701dd7f26 | 131 | * kai++; |
aoikoizumi | 0:a40701dd7f26 | 132 | * if(kai>1000){ |
aoikoizumi | 0:a40701dd7f26 | 133 | * printf("omega=%f\r\n",EC.getOmega()); |
aoikoizumi | 0:a40701dd7f26 | 134 | * kai=0; |
aoikoizumi | 0:a40701dd7f26 | 135 | * } |
aoikoizumi | 0:a40701dd7f26 | 136 | * } |
aoikoizumi | 0:a40701dd7f26 | 137 | * motor.stop() |
aoikoizumi | 0:a40701dd7f26 | 138 | * while(1); |
aoikoizumi | 0:a40701dd7f26 | 139 | * } |
aoikoizumi | 0:a40701dd7f26 | 140 | * @endcode |
aoikoizumi | 0:a40701dd7f26 | 141 | * |
aoikoizumi | 0:a40701dd7f26 | 142 | * @section CAUTION(printfの負荷について) |
aoikoizumi | 0:a40701dd7f26 | 143 | * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。 |
aoikoizumi | 0:a40701dd7f26 | 144 | * |
aoikoizumi | 0:a40701dd7f26 | 145 | * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。 |
aoikoizumi | 0:a40701dd7f26 | 146 | * |
aoikoizumi | 0:a40701dd7f26 | 147 | * (どんなプログラムにも言えるが、)そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。 |
aoikoizumi | 0:a40701dd7f26 | 148 | */ |
aoikoizumi | 0:a40701dd7f26 | 149 | void Sc(double target_omega); |
aoikoizumi | 0:a40701dd7f26 | 150 | /** モーター停止用関数 |
aoikoizumi | 0:a40701dd7f26 | 151 | * dutyを0にする |
aoikoizumi | 0:a40701dd7f26 | 152 | */ |
aoikoizumi | 0:a40701dd7f26 | 153 | void stop(); |
aoikoizumi | 0:a40701dd7f26 | 154 | /** duty(-1 ~ 1)比でモーターを回転させる関数 |
aoikoizumi | 0:a40701dd7f26 | 155 | * 負の値を入れたら逆回転する |
aoikoizumi | 0:a40701dd7f26 | 156 | * @param duty 出力duty比 |
aoikoizumi | 0:a40701dd7f26 | 157 | * @section SAMPLE |
aoikoizumi | 0:a40701dd7f26 | 158 | * @code |
aoikoizumi | 0:a40701dd7f26 | 159 | * #include "mbed.h" |
aoikoizumi | 0:a40701dd7f26 | 160 | * #include "EC.h" //Encoderライブラリをインクルード |
aoikoizumi | 0:a40701dd7f26 | 161 | * #include "SpeedController.h" //SpeedControlライブラリをインクルード |
aoikoizumi | 0:a40701dd7f26 | 162 | * #define RESOLUTION 500 |
aoikoizumi | 0:a40701dd7f26 | 163 | * Ec4multi EC(p11,p12,RESOLUTION); |
aoikoizumi | 0:a40701dd7f26 | 164 | * SpeedControl motor(p21,p22,50,EC); |
aoikoizumi | 0:a40701dd7f26 | 165 | * int main(){ |
aoikoizumi | 0:a40701dd7f26 | 166 | * while(1){ |
aoikoizumi | 0:a40701dd7f26 | 167 | * motor.turn(0.5); //duty0.5で正転 |
aoikoizumi | 0:a40701dd7f26 | 168 | * wait(1); |
aoikoizumi | 0:a40701dd7f26 | 169 | * motor.turn(0); //停止 |
aoikoizumi | 0:a40701dd7f26 | 170 | * wait(1); |
aoikoizumi | 0:a40701dd7f26 | 171 | * motor.turn(-0.5); //duty0.5で逆転 |
aoikoizumi | 0:a40701dd7f26 | 172 | * wait(1); |
aoikoizumi | 0:a40701dd7f26 | 173 | * motor.turn(0); //停止 |
aoikoizumi | 0:a40701dd7f26 | 174 | * wait(1); |
aoikoizumi | 0:a40701dd7f26 | 175 | * } |
aoikoizumi | 0:a40701dd7f26 | 176 | * } |
aoikoizumi | 0:a40701dd7f26 | 177 | * @endcode |
aoikoizumi | 0:a40701dd7f26 | 178 | */ |
aoikoizumi | 0:a40701dd7f26 | 179 | void turn(double duty); |
aoikoizumi | 0:a40701dd7f26 | 180 | ///Encoderや速度制御の値をresetする関数 |
aoikoizumi | 0:a40701dd7f26 | 181 | void reset(); |
aoikoizumi | 0:a40701dd7f26 | 182 | }; |
aoikoizumi | 0:a40701dd7f26 | 183 | |
aoikoizumi | 0:a40701dd7f26 | 184 | #endif |