SpeedController___2_25 - 2月25日

ロボステ6期 » Code » SpeedController___2_25

2月25日

Dependencies: Encoder

Dependents: NHK2020-m2-2-ros_2_25

SpeedController.h@5:fddbe92ca86f, 2019-08-10 (annotated)

Committer:: MazeTaka
Date:: Sat Aug 10 04:13:52 2019 +0000
Revision:: 5:fddbe92ca86f
Parent:: 4:ee0ac8415639
Child:: 6:d110c276aa5a

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	0:de44976b7670	28	* @param pwm_F モーターを正転させるpwmピン名
MazeTaka	0:de44976b7670	29	* @param pwm_B モーターを後転させるpwmピン名
MazeTaka	0:de44976b7670	30	* @param us モーターのpwm周期[us]
MazeTaka	0:de44976b7670	31	* @param ec Encoder classのポインタ
MazeTaka	0:de44976b7670	32	* モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
MazeTaka	0:de44976b7670	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	5:fddbe92ca86f	48	* @senction SAMPLE
MazeTaka	5:fddbe92ca86f	49	* @code
MazeTaka	5:fddbe92ca86f	50	* SpeedControl motor(p21,p22,50,EC);
MazeTaka	5:fddbe92ca86f	51	* int main(){
MazeTaka	5:fddbe92ca86f	52	* motor.period_us(50);
MazeTaka	5:fddbe92ca86f	53	* while(1){
MazeTaka	5:fddbe92ca86f	54	* @endcode
MazeTaka	5:fddbe92ca86f	55	*/
MazeTaka	5:fddbe92ca86f	56	void period_us(int us);
MazeTaka	5:fddbe92ca86f	57	/** PIDパラメータ設定関数
MazeTaka	5:fddbe92ca86f	58	*
MazeTaka	5:fddbe92ca86f	59	* 引数はそれぞれp,dパラメータ
MazeTaka	5:fddbe92ca86f	60	*
MazeTaka	5:fddbe92ca86f	61	* パラメーター値は実験的に頑張って求めるしかない
MazeTaka	5:fddbe92ca86f	62	* @param kp p制御パラメータ
MazeTaka	5:fddbe92ca86f	63	* @param kd d制御パラメータ
MazeTaka	5:fddbe92ca86f	64	* @senction SAMPLE
MazeTaka	5:fddbe92ca86f	65	* @code
MazeTaka	5:fddbe92ca86f	66	* SpeedControl motor(p21,p22,50,EC);
MazeTaka	5:fddbe92ca86f	67	* int main(){
MazeTaka	5:fddbe92ca86f	68	* motor.setPDparam(0.1,0.001);
MazeTaka	5:fddbe92ca86f	69	* while(1){
MazeTaka	5:fddbe92ca86f	70	* @endcode
MazeTaka	5:fddbe92ca86f	71	*/
MazeTaka	5:fddbe92ca86f	72	void setPDparam(double kp,double kd);
MazeTaka	5:fddbe92ca86f	73
MazeTaka	5:fddbe92ca86f	74	/** 角速度・duty比の関係式の設定関数
MazeTaka	5:fddbe92ca86f	75	* 横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する
MazeTaka	5:fddbe92ca86f	76	* @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時)
MazeTaka	5:fddbe92ca86f	77	* @param df 角速度[rad/s]とduty比の関係式の切片(正転時)
MazeTaka	5:fddbe92ca86f	78	* @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時)
MazeTaka	5:fddbe92ca86f	79	* @param db 角速度[rad/s]とduty比の関係式の切片(逆転時)
MazeTaka	5:fddbe92ca86f	80	* @senction SAMPLE
MazeTaka	5:fddbe92ca86f	81	* @code
MazeTaka	5:fddbe92ca86f	82	* SpeedControl motor(p21,p22,50,EC);
MazeTaka	5:fddbe92ca86f	83	* int main(){
MazeTaka	5:fddbe92ca86f	84	* motor.setEquation(0.02,0.0001,-0.02,0.0001);
MazeTaka	5:fddbe92ca86f	85	* while(1){
MazeTaka	5:fddbe92ca86f	86	* @endcode
MazeTaka	5:fddbe92ca86f	87	*/
MazeTaka	5:fddbe92ca86f	88	void setEquation(double cf,double df,double cb,double db);
MazeTaka	5:fddbe92ca86f	89	/** 出力duty比の上限を決める関数
MazeTaka	5:fddbe92ca86f	90	* 0 ~ 0.95の範囲内で設定する
MazeTaka	5:fddbe92ca86f	91	* @param duty_limit 出力duty比の上限
MazeTaka	5:fddbe92ca86f	92	*/
MazeTaka	5:fddbe92ca86f	93	void setDutyLimit(double duty_limit);
MazeTaka	0:de44976b7670	94	/** 速度制御関数、引数は目標速度
MazeTaka	0:de44976b7670	95	*
MazeTaka	0:de44976b7670	96	* この目標角加速度になるようにモーターを回転させることができる
MazeTaka	0:de44976b7670	97	* 負の数を代入すれば逆回転することができる
MazeTaka	0:de44976b7670	98	*
MazeTaka	0:de44976b7670	99	* @param target_omega 目標角速度
MazeTaka	4:ee0ac8415639	100	* @section SAMPLE
MazeTaka	4:ee0ac8415639	101	* @code
MazeTaka	4:ee0ac8415639	102	* //プログラム例
MazeTaka	4:ee0ac8415639	103	* #include "mbed.h"
MazeTaka	4:ee0ac8415639	104	* #include "EC.h" //Encoderライブラリをインクルード
MazeTaka	4:ee0ac8415639	105	* #include "SpeedController.h" //SpeedControlライブラリをインクルード
MazeTaka	4:ee0ac8415639	106	* #define RESOLUTION 500
MazeTaka	4:ee0ac8415639	107	* Ec4multi EC(p11,p12,RESOLUTION);
MazeTaka	4:ee0ac8415639	108	* SpeedControl motor(p21,p22,50,EC);
MazeTaka	4:ee0ac8415639	109	* int main(){
MazeTaka	4:ee0ac8415639	110	* motor.setEquation(0.0062,0.0045,-0.0061,0.0091); //求めたC,Dの値を設定
MazeTaka	4:ee0ac8415639	111	* motor.setPDparam(0.1,0.0); //PIDの係数を設定
MazeTaka	4:ee0ac8415639	112	* int kai=0;
MazeTaka	4:ee0ac8415639	113	* for(int i=0;i<100000000;i++){
MazeTaka	4:ee0ac8415639	114	* motor.Sc(10); //10[rad/s]でモーターを回転
MazeTaka	4:ee0ac8415639	115	* kai++;
MazeTaka	4:ee0ac8415639	116	* if(kai>1000){
MazeTaka	4:ee0ac8415639	117	* printf("omega=%f\r\n",EC.getOmega());
MazeTaka	4:ee0ac8415639	118	* kai=0;
MazeTaka	4:ee0ac8415639	119	* }
MazeTaka	4:ee0ac8415639	120	* }
MazeTaka	4:ee0ac8415639	121	* motor.stop()
MazeTaka	4:ee0ac8415639	122	* while(1);
MazeTaka	4:ee0ac8415639	123	* }
MazeTaka	4:ee0ac8415639	124	* @endcode
MazeTaka	0:de44976b7670	125	*
MazeTaka	5:fddbe92ca86f	126	* @section CAUTION（printfの負荷について）
MazeTaka	0:de44976b7670	127	* printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
MazeTaka	5:fddbe92ca86f	128	*
MazeTaka	0:de44976b7670	129	* なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
MazeTaka	5:fddbe92ca86f	130	*
MazeTaka	0:de44976b7670	131	* （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
MazeTaka	0:de44976b7670	132	*/
MazeTaka	0:de44976b7670	133	void Sc(double target_omega);
MazeTaka	0:de44976b7670	134	/** モーター停止用関数
MazeTaka	0:de44976b7670	135	* dutyを0にする
MazeTaka	0:de44976b7670	136	*/
MazeTaka	0:de44976b7670	137	void stop();
MazeTaka	0:de44976b7670	138	/** duty(-1 ~ 1)比でモーターを回転させる関数
MazeTaka	0:de44976b7670	139	* 負の値を入れたら逆回転する
MazeTaka	2:77256385cf03	140	* @param duty 出力duty比
MazeTaka	4:ee0ac8415639	141	* @section SAMPLE
MazeTaka	4:ee0ac8415639	142	* @code
MazeTaka	4:ee0ac8415639	143	* #include "mbed.h"
MazeTaka	4:ee0ac8415639	144	* #include "EC.h" //Encoderライブラリをインクルード
MazeTaka	4:ee0ac8415639	145	* #include "SpeedController.h" //SpeedControlライブラリをインクルード
MazeTaka	4:ee0ac8415639	146	* #define RESOLUTION 500
MazeTaka	4:ee0ac8415639	147	* Ec4multi EC(p11,p12,RESOLUTION);
MazeTaka	4:ee0ac8415639	148	* SpeedControl motor(p21,p22,50,EC);
MazeTaka	4:ee0ac8415639	149	* int main(){
MazeTaka	4:ee0ac8415639	150	* while(1){
MazeTaka	5:fddbe92ca86f	151	* motor.turn(0.5); //duty0.5で正転
MazeTaka	4:ee0ac8415639	152	* wait(1);
MazeTaka	5:fddbe92ca86f	153	* motor.turn(0); //停止
MazeTaka	4:ee0ac8415639	154	* wait(1);
MazeTaka	5:fddbe92ca86f	155	* motor.turn(-0.5); //duty0.5で逆転
MazeTaka	4:ee0ac8415639	156	* wait(1);
MazeTaka	5:fddbe92ca86f	157	* motor.turn(0); //停止
MazeTaka	4:ee0ac8415639	158	* wait(1);
MazeTaka	4:ee0ac8415639	159	* }
MazeTaka	4:ee0ac8415639	160	* }
MazeTaka	4:ee0ac8415639	161	* @endcode
MazeTaka	0:de44976b7670	162	*/
MazeTaka	0:de44976b7670	163	void turn(double duty);
MazeTaka	0:de44976b7670	164	///Encoderや速度制御の値をresetする関数
MazeTaka	0:de44976b7670	165	void reset();
MazeTaka	0:de44976b7670	166	};
MazeTaka	0:de44976b7670	167
MazeTaka	0:de44976b7670	168	#endif
MazeTaka	0:de44976b7670	169

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	25 Feb 2020
Imports:	1
Forks:	0
Commits:	12
Dependents:	1
Dependencies:	1
Followers:	11

SpeedController.h@5:fddbe92ca86f, 2019-08-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning