ロボステ6期
/
NHK2020-arm-sub4
12/20 mainmode3
SpeedController/SpeedController.h
- Committer:
- aoikoizumi
- Date:
- 2019-12-20
- Revision:
- 3:9cef6e58a462
- Parent:
- 0:6e2abd0956f1
File content as of revision 3:9cef6e58a462:
#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