ロボステ6期 / Mbed 2 deprecated ball_snatch

e

Dependencies:   mbed Encoder_

SpeedController/SpeedController.h@0:a40701dd7f26, 2019-12-13 (annotated)

Committer:
aoikoizumi
Date:
Fri Dec 13 06:49:35 2019 +0000
Revision:
0:a40701dd7f26
e

Who changed what in which revision?

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