SpeedController___2_25 - 2月25日

ロボステ6期 » Code » SpeedController___2_25

2月25日

Dependencies: Encoder

Dependents: NHK2020-m2-2-ros_2_25

SpeedController.h@4:ee0ac8415639, 2019-08-09 (annotated)

Committer:: MazeTaka
Date:: Fri Aug 09 07:15:35 2019 +0000
Revision:: 4:ee0ac8415639
Parent:: 3:58a298386879
Child:: 5:fddbe92ca86f

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

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@4:ee0ac8415639, 2019-08-09 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning