ロボステ6期 / SpeedController___2_25

2月25日

Dependencies:   Encoder

Dependents:   NHK2020-m2-2-ros_2_25

SpeedController.h@11:b3e2ec75a4d7, 2020-02-25 (annotated)

Committer:
yuki0701
Date:
Tue Feb 25 01:24:31 2020 +0000
Revision:
11:b3e2ec75a4d7
Parent:
7:b1708b8819f1 
a

Who changed what in which revision?

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