ロボステ6期 / Mbed 2 deprecated NHK2020-arm-sub4

12/20 mainmode3

Dependencies:   mbed Encoder_

SpeedController/SpeedController.h@3:9cef6e58a462, 2019-12-20 (annotated)

Committer:
aoikoizumi
Date:
Fri Dec 20 10:21:47 2019 +0000
Revision:
3:9cef6e58a462
Parent:
0:6e2abd0956f1 
testmode_3 12/20

Who changed what in which revision?

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