ロボステ6期
/
NHK2020-arm-sub4
12/20 mainmode3
Diff: SpeedController/SpeedController.h
- Revision:
- 0:6e2abd0956f1
diff -r 000000000000 -r 6e2abd0956f1 SpeedController/SpeedController.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpeedController/SpeedController.h Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,184 @@ +#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_; + 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); + + + + /**モーターのpwm周期の設定用関数 + * @param s モーターのpwm周期[s] + */ + void period(double s); + /**モーターのpwm周期の設定用関数 + * @param ms モーターのpwm周期[ms] + */ + void period_ms(int ms); + /**モーターのpwm周期の設定用関数 + * @param us モーターのpwm周期[us] + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.period_us(50); + * while(1){ + * @endcode + */ + void period_us(int us); + /** PIDパラメータ設定関数 + * + * 引数はそれぞれp,dパラメータ + * + * パラメーター値は実験的に頑張って求めるしかない + * @param kp p制御パラメータ + * @param kd d制御パラメータ + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setPDparam(0.1,0.001); + * while(1){ + * @endcode + */ + 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比の関係式の切片(逆転時) + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setEquation(0.02,0.0001,-0.02,0.0001); + * while(1){ + * @endcode + */ + void setEquation(double cf,double df,double cb,double db); + /** 出力duty比の上限を決める関数 + * 0 ~ 0.95の範囲内で設定する + * @param duty_limit 出力duty比の上限 + * + * @senction SAMPLE + * @code + * Ec4multi EC(p11,p12,500); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setDutyLimit(0.5); //duty比の上限を0.5に設定 + * while(1){ + * @endcode + */ + void setDutyLimit(double duty_limit); + /** 速度制御関数、引数は目標速度 + * + * この目標角加速度になるようにモーターを回転させることができる + * 負の数を代入すれば逆回転することができる + * + * @param target_omega 目標角速度 + * + * @section SAMPLE + * @code + * //プログラム例 + * #include "mbed.h" + * #include "EC.h" //Encoderライブラリをインクルード + * #include "SpeedController.h" //SpeedControlライブラリをインクルード + * #define RESOLUTION 500 + * Ec4multi EC(p11,p12,RESOLUTION); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * motor.setEquation(0.0062,0.0045,-0.0061,0.0091); //求めたC,Dの値を設定 + * motor.setPDparam(0.1,0.0); //PIDの係数を設定 + * int kai=0; + * for(int i=0;i<100000000;i++){ + * motor.Sc(10); //10[rad/s]でモーターを回転 + * kai++; + * if(kai>1000){ + * printf("omega=%f\r\n",EC.getOmega()); + * kai=0; + * } + * } + * motor.stop() + * while(1); + * } + * @endcode + * + * @section CAUTION(printfの負荷について) + * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。 + * + * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。 + * + * (どんなプログラムにも言えるが、)そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。 + */ + void Sc(double target_omega); + /** モーター停止用関数 + * dutyを0にする + */ + void stop(); + /** duty(-1 ~ 1)比でモーターを回転させる関数 + * 負の値を入れたら逆回転する + * @param duty 出力duty比 + * @section SAMPLE + * @code + * #include "mbed.h" + * #include "EC.h" //Encoderライブラリをインクルード + * #include "SpeedController.h" //SpeedControlライブラリをインクルード + * #define RESOLUTION 500 + * Ec4multi EC(p11,p12,RESOLUTION); + * SpeedControl motor(p21,p22,50,EC); + * int main(){ + * while(1){ + * motor.turn(0.5); //duty0.5で正転 + * wait(1); + * motor.turn(0); //停止 + * wait(1); + * motor.turn(-0.5); //duty0.5で逆転 + * wait(1); + * motor.turn(0); //停止 + * wait(1); + * } + * } + * @endcode + */ + void turn(double duty); + ///Encoderや速度制御の値をresetする関数 + void reset(); +}; + +#endif \ No newline at end of file