riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

SpeedController.h@35:b47330f0dec2, 2018-11-03 (annotated)

Committer:
aoikoizumi
Date:
Sat Nov 03 23:09:50 2018 +0000
Revision:
35:b47330f0dec2
Parent:
32:297384f9d261 
?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 32:297384f9d261 1 #ifndef INCLUDED_SPEEDCONTROLLER_SPEEDCONTROLLER_H
shimizuta 32:297384f9d261 2 #define INCLUDED_SPEEDCONTROLLER_SPEEDCONTROLLER_H
shimizuta 32:297384f9d261 3 #include "EC.h"
shimizuta 32:297384f9d261 4 /** @class SpeedControl
shimizuta 32:297384f9d261 5 *速度制御用ライブラリ
shimizuta 32:297384f9d261 6 *
shimizuta 32:297384f9d261 7 * サンプルプログラム
shimizuta 32:297384f9d261 8 * @code
shimizuta 32:297384f9d261 9 * //サンプルプログラム 差動二輪駆動ロボットを想定
shimizuta 32:297384f9d261 10 * //直進・後退・右旋回・左旋回が可能
shimizuta 32:297384f9d261 11 * #include "mbed.h"
shimizuta 32:297384f9d261 12 * #include "SpeedController.h"
shimizuta 32:297384f9d261 13 *
shimizuta 32:297384f9d261 14 * #define BASIC_SPEED 20 //モーターはこの角速度で駆動させる
shimizuta 32:297384f9d261 15 *
shimizuta 32:297384f9d261 16 * SpeedControl motorR(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //右輪
shimizuta 32:297384f9d261 17 * SpeedControl motorL(PF_1,PA_11,NC,1048,0.05,PA_9,PA_10); //左輪
shimizuta 32:297384f9d261 18 * Ticker motor_tick; //角速度計算用ticker
shimizuta 32:297384f9d261 19 * Serial pc(USBTX,USBRX);
shimizuta 32:297384f9d261 20 *
shimizuta 32:297384f9d261 21 * void calOmega() //角速度計算関数
shimizuta 32:297384f9d261 22 * {
shimizuta 32:297384f9d261 23 * motorR.CalOmega();
shimizuta 32:297384f9d261 24 * motorL.CalOmega();
shimizuta 32:297384f9d261 25 * }
shimizuta 32:297384f9d261 26 *
shimizuta 32:297384f9d261 27 * int main(void){
shimizuta 32:297384f9d261 28 * motor_tick.attach(&calOmega,0.05);
shimizuta 32:297384f9d261 29 * //Cの値を設定
shimizuta 32:297384f9d261 30 * motorR.setDOconstant(46.3);
shimizuta 32:297384f9d261 31 * motorL.setDOconstant(47.2);
shimizuta 32:297384f9d261 32 * //pd係数を設定
shimizuta 32:297384f9d261 33 * motorR.setPDparam(0.1,0);
shimizuta 32:297384f9d261 34 * motorR.setPDparam(0.12,0);
shimizuta 32:297384f9d261 35 *
shimizuta 32:297384f9d261 36 * int kai=0;
shimizuta 32:297384f9d261 37 * double target_R=0,target_L=0;
shimizuta 32:297384f9d261 38 * char status[5][10]={"STOP","FORWARD","BACKWARD","RIGHT","LEFT"};
shimizuta 32:297384f9d261 39 * int status_num=0; //初期状態は停止
shimizuta 32:297384f9d261 40 *
shimizuta 32:297384f9d261 41 * while(1) {
shimizuta 32:297384f9d261 42 * motorR.Sc(target_R);
shimizuta 32:297384f9d261 43 * motorL.Sc(target_L); //target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
shimizuta 32:297384f9d261 44 *
shimizuta 32:297384f9d261 45 * if(kai>=500) {
shimizuta 32:297384f9d261 46 * pc.printf("status=%s omega_R=%f omega_L=%f \r\n",status[status_num],motorR.getOmega(),motorL.getOmega());
shimizuta 32:297384f9d261 47 * //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
shimizuta 32:297384f9d261 48 * kai=0;
shimizuta 32:297384f9d261 49 * }
shimizuta 32:297384f9d261 50 *
shimizuta 32:297384f9d261 51 * if(pc.readable()) {
shimizuta 32:297384f9d261 52 * char sel=pc.getc();
shimizuta 32:297384f9d261 53 *
shimizuta 32:297384f9d261 54 * switch (sel) {
shimizuta 32:297384f9d261 55 * case 's': //停止
shimizuta 32:297384f9d261 56 * motorR.stop();
shimizuta 32:297384f9d261 57 * motorL.stop();
shimizuta 32:297384f9d261 58 * target_R=0;
shimizuta 32:297384f9d261 59 * target_L=0;
shimizuta 32:297384f9d261 60 * status_num=0;
shimizuta 32:297384f9d261 61 * break;
shimizuta 32:297384f9d261 62 * case 'i': //前進
shimizuta 32:297384f9d261 63 * target_R=BASIC_SPEED;
shimizuta 32:297384f9d261 64 * target_L=BASIC_SPEED;
shimizuta 32:297384f9d261 65 * status_num=1;
shimizuta 32:297384f9d261 66 * break;
shimizuta 32:297384f9d261 67 * case 'm': //後退
shimizuta 32:297384f9d261 68 * target_R=(-1)*BASIC_SPEED;
shimizuta 32:297384f9d261 69 * target_L=(-1)*BASIC_SPEED;
shimizuta 32:297384f9d261 70 * status_num=2;
shimizuta 32:297384f9d261 71 * break;
shimizuta 32:297384f9d261 72 * case 'k': //右旋回
shimizuta 32:297384f9d261 73 * motorR.stop();
shimizuta 32:297384f9d261 74 * target_R=0;
shimizuta 32:297384f9d261 75 * target_L=BASIC_SPEED;
shimizuta 32:297384f9d261 76 * status_num=3;
shimizuta 32:297384f9d261 77 * break;
shimizuta 32:297384f9d261 78 * case 'j': //左旋回
shimizuta 32:297384f9d261 79 * motorL.stop();
shimizuta 32:297384f9d261 80 * target_R=BASIC_SPEED;
shimizuta 32:297384f9d261 81 * target_L=0;
shimizuta 32:297384f9d261 82 * status_num=4;
shimizuta 32:297384f9d261 83 * break;
shimizuta 32:297384f9d261 84 * default:
shimizuta 32:297384f9d261 85 * break;
shimizuta 32:297384f9d261 86 * }
shimizuta 32:297384f9d261 87 * if(sel=='q'){
shimizuta 32:297384f9d261 88 * break; //qを押したら左右のモーターを停止
shimizuta 32:297384f9d261 89 * }
shimizuta 32:297384f9d261 90 * }
shimizuta 32:297384f9d261 91 * kai++;
shimizuta 32:297384f9d261 92 * }
shimizuta 32:297384f9d261 93 * motorR.stop();
shimizuta 32:297384f9d261 94 * motorL.stop();
shimizuta 32:297384f9d261 95 *
shimizuta 32:297384f9d261 96 * return 0;
shimizuta 32:297384f9d261 97 * }
shimizuta 32:297384f9d261 98 * @endcode
shimizuta 32:297384f9d261 99 * @section 基本的な速度制御の実用までの流れ
shimizuta 32:297384f9d261 100 *
shimizuta 32:297384f9d261 101 * 1. モーター・エンコーダ・モータードライバ・マイコン・バッテリーを適切につなげる
shimizuta 32:297384f9d261 102 *
shimizuta 32:297384f9d261 103 * 接続のイメージ↓
shimizuta 32:297384f9d261 104 *
shimizuta 32:297384f9d261 105 * https://www.fastpic.jp/images.php?file=0391548523.png
shimizuta 32:297384f9d261 106 *
shimizuta 32:297384f9d261 107 * 注意！！！ リポ用のスイッチと電圧計を必ずつけること！！！
shimizuta 32:297384f9d261 108 *
shimizuta 32:297384f9d261 109 * /
shimizuta 32:297384f9d261 110 *
shimizuta 32:297384f9d261 111 * 2. 実際にモーターを回転させてみて不備なく動作するか確認する
shimizuta 32:297384f9d261 112 *
shimizuta 32:297384f9d261 113 * 確認事項
shimizuta 32:297384f9d261 114 *
shimizuta 32:297384f9d261 115 * ・モーターが回るか、duty比によって加速・減速するか
shimizuta 32:297384f9d261 116 *
shimizuta 32:297384f9d261 117 * ・エンコーダの値が取れているか
shimizuta 32:297384f9d261 118 *
shimizuta 32:297384f9d261 119 * ・モーターの回転方向とエンコーダの加算減算が対応しているか (プログラム内で定義した正・負回転とエンコーダの増減が対応しているかということ)
shimizuta 32:297384f9d261 120 *
shimizuta 32:297384f9d261 121 * などなど。コードは下の一つ目のものを使えばいい
shimizuta 32:297384f9d261 122 *
shimizuta 32:297384f9d261 123 * /
shimizuta 32:297384f9d261 124 *
shimizuta 32:297384f9d261 125 * 3. Cの値を見つける
shimizuta 32:297384f9d261 126 *
shimizuta 32:297384f9d261 127 * このライブラリでは、「モーターにかかる電圧（duty比）と回転速度はある程度比例の関係にあるだろう」という考え方をしている（もちろん実際のモデルはもっと複雑である）。
shimizuta 32:297384f9d261 128 *
shimizuta 32:297384f9d261 129 * このライブラリにおける速度制御のアルゴリズムとしては、あらかじめ求めたこの比例係数を用いて、目標角速度からだいたいのduty比を算出し、そこからpd制御をかけるという形をとっている。
shimizuta 32:297384f9d261 130 *
shimizuta 32:297384f9d261 131 * Cとはこの比例係数のことである。一応式としてあらわすと、
shimizuta 32:297384f9d261 132 *
shimizuta 32:297384f9d261 133 * （回転速度）＝C×（duty比）
shimizuta 32:297384f9d261 134 *
shimizuta 32:297384f9d261 135 * となる。このCの値はモーターやギアユニットによって固有の値であり、それぞれのモーターにおいて求める必要がある。
shimizuta 32:297384f9d261 136 *
shimizuta 32:297384f9d261 137 * 具体的なこの係数の求め方は、duty比を0.05くらいずつで変化させ、その時のduty比と回転角速度を記録し、excelからグラフの傾きを求めればよい。
shimizuta 32:297384f9d261 138 *
shimizuta 32:297384f9d261 139 * こんな感じになるよっていう画像↓
shimizuta 32:297384f9d261 140 *
shimizuta 32:297384f9d261 141 * https://www.fastpic.jp/images.php?file=2416798781.gif
shimizuta 32:297384f9d261 142 *
shimizuta 32:297384f9d261 143 * コードは下の一つ目のものを使えばいい
shimizuta 32:297384f9d261 144 *
shimizuta 32:297384f9d261 145 * /
shimizuta 32:297384f9d261 146 *
shimizuta 32:297384f9d261 147 * 4. pd係数を見つける
shimizuta 32:297384f9d261 148 *
shimizuta 32:297384f9d261 149 * 上記の通り、最終的にはpd制御（精確にはi+pd制御）によって目標角速度に収束させる。
shimizuta 32:297384f9d261 150 *
shimizuta 32:297384f9d261 151 * ここではそのpd制御の操作量を決定する係数を見つける。これも実験的に求めるほかなく、値を変えて挙動を見てを繰り返して最適な係数を見つける。
shimizuta 32:297384f9d261 152 *
shimizuta 32:297384f9d261 153 * 値の評価は、目標に達するまでの速さ、オーバーシュートの少なさ、収束後の値の振動の小ささなどである。
shimizuta 32:297384f9d261 154 *
shimizuta 32:297384f9d261 155 * 重要度的にはp制御＞d制御（場合によってはd制御が不必要なことも）なので、まずdの係数を0にした状態で先にpの係数を見つける。
shimizuta 32:297384f9d261 156 *
shimizuta 32:297384f9d261 157 * コードは下の二つ目のものを使えばいい
shimizuta 32:297384f9d261 158 *
shimizuta 32:297384f9d261 159 * @code
shimizuta 32:297384f9d261 160 * //2,3で使えるサンプルコード
shimizuta 32:297384f9d261 161 * //入力はTera Termで行う
shimizuta 32:297384f9d261 162 * #include "mbed.h"
shimizuta 32:297384f9d261 163 * #include "SpeedController.h"
shimizuta 32:297384f9d261 164 *
shimizuta 32:297384f9d261 165 * SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5);
shimizuta 32:297384f9d261 166 * Serial pc(USBTX,USBRX);
shimizuta 32:297384f9d261 167 * Ticker motor_tick;
shimizuta 32:297384f9d261 168 *
shimizuta 32:297384f9d261 169 * void calOmega(){
shimizuta 32:297384f9d261 170 * motor.CalOmega(); //角速度計算用
shimizuta 32:297384f9d261 171 * }
shimizuta 32:297384f9d261 172 *
shimizuta 32:297384f9d261 173 * int main(void){
shimizuta 32:297384f9d261 174 * motor_tick.attach(calOmega,0.05);
shimizuta 32:297384f9d261 175 * int loop_time=0;
shimizuta 32:297384f9d261 176 * float in_duty=0;
shimizuta 32:297384f9d261 177 * bool print=false;
shimizuta 32:297384f9d261 178 *
shimizuta 32:297384f9d261 179 * while(1){
shimizuta 32:297384f9d261 180 * loop_time++;
shimizuta 32:297384f9d261 181 *
shimizuta 32:297384f9d261 182 * if(in_duty>0) motor.turnF(in_duty); //in_dutyが正のとき正回転（このプログラムではPA_8で駆動）
shimizuta 32:297384f9d261 183 * else motor.turnB((-1)*in_duty); //in_dutyが負のとき逆回転（このプログラムではPB_5で駆動）
shimizuta 32:297384f9d261 184 *
shimizuta 32:297384f9d261 185 *
shimizuta 32:297384f9d261 186 * if(pc.readable()){
shimizuta 32:297384f9d261 187 * char sel=pc.getc();
shimizuta 32:297384f9d261 188 * if(sel=='s'){
shimizuta 32:297384f9d261 189 * motor.stop();
shimizuta 32:297384f9d261 190 * } else if(sel=='f'){
shimizuta 32:297384f9d261 191 * in_duty+=(float)0.05; //fを押すとduty比がo.o5ずつあがる
shimizuta 32:297384f9d261 192 * pc.printf("duty=%f\r\n",in_duty);
shimizuta 32:297384f9d261 193 * } else if(sel=='b'){
shimizuta 32:297384f9d261 194 * in_duty-=(float)0.05; //bを押すとduty比がo.o5ずつさがる
shimizuta 32:297384f9d261 195 * pc.printf("duty=%f\r\n",in_duty);
shimizuta 32:297384f9d261 196 * } else if(sel=='p'){
shimizuta 32:297384f9d261 197 * print=!print; //pを押すと表示を停止/再開する
shimizuta 32:297384f9d261 198 * }
shimizuta 32:297384f9d261 199 * }
shimizuta 32:297384f9d261 200 *
shimizuta 32:297384f9d261 201 * if(loop_time%1000==0){
shimizuta 32:297384f9d261 202 * if(print) pc.printf("count=%d omega=%f dutyF=%f dutyB=%f\r\n",motor.getCount(),motor.getOmega(),(double)motor.pwm_F_,(double)motor.pwm_B_);
shimizuta 32:297384f9d261 203 * } //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示
shimizuta 32:297384f9d261 204 * }
shimizuta 32:297384f9d261 205 * }
shimizuta 32:297384f9d261 206 * @endcode
shimizuta 32:297384f9d261 207 *
shimizuta 32:297384f9d261 208 * @code
shimizuta 32:297384f9d261 209 * //PDの係数を探したいときに使えるサンプル
shimizuta 32:297384f9d261 210 * //入力はTera Termで行う
shimizuta 32:297384f9d261 211 * #include "mbed.h"
shimizuta 32:297384f9d261 212 * #include "SpeedController.h"
shimizuta 32:297384f9d261 213 *
shimizuta 32:297384f9d261 214 * #define TARGET_OMEGA 20 //目標角速度を20rad/sに設定
shimizuta 32:297384f9d261 215 *
shimizuta 32:297384f9d261 216 * SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定
shimizuta 32:297384f9d261 217 * Ticker motor_tick; //角速度計算用ticker
shimizuta 32:297384f9d261 218 * Serial pc(USBTX,USBRX);
shimizuta 32:297384f9d261 219 *
shimizuta 32:297384f9d261 220 * void calOmega() //角速度計算関数
shimizuta 32:297384f9d261 221 * {
shimizuta 32:297384f9d261 222 * motor.CalOmega();
shimizuta 32:297384f9d261 223 * }
shimizuta 32:297384f9d261 224 *
shimizuta 32:297384f9d261 225 * int main()
shimizuta 32:297384f9d261 226 * {
shimizuta 32:297384f9d261 227 * motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
shimizuta 32:297384f9d261 228 * motor.setDOconstant(46.3); //求めたCの値を設定
shimizuta 32:297384f9d261 229 *
shimizuta 32:297384f9d261 230 * int kai=0;
shimizuta 32:297384f9d261 231 * double target_omega=0;
shimizuta 32:297384f9d261 232 * double P=0.0,D=0.0;
shimizuta 32:297384f9d261 233 *
shimizuta 32:297384f9d261 234 * while(1) {
shimizuta 32:297384f9d261 235 * motor.Sc(target_omega); //速度制御のコア関数
shimizuta 32:297384f9d261 236 * if(kai>=500) {
shimizuta 32:297384f9d261 237 * pc.printf("count=%d omega=%f target_omega=%f dutyF=%f dutyB=%f P=%f D=%f\r\n",motor.getCount(),motor.getOmega(),target_omega,(double)motor.pwm_F_,(double)motor.pwm_B_,P,D);
shimizuta 32:297384f9d261 238 * //エンコーダのカウント、角速度、目標角速度、正方向出力duty比、負方向出力duty比、Pの係数、Dの係数を表示
shimizuta 32:297384f9d261 239 * kai=0;
shimizuta 32:297384f9d261 240 * }
shimizuta 32:297384f9d261 241 * //係数探しの流れとしては、係数変更→速度制御開始→収束具合を観察・比較→モーター停止の繰り返し
shimizuta 32:297384f9d261 242 * if(pc.readable()) {
shimizuta 32:297384f9d261 243 * char sel=pc.getc();
shimizuta 32:297384f9d261 244 *
shimizuta 32:297384f9d261 245 * switch (sel) {
shimizuta 32:297384f9d261 246 * case 'k': //kでpの係数に0.01加算
shimizuta 32:297384f9d261 247 * P+=0.01;
shimizuta 32:297384f9d261 248 * break;
shimizuta 32:297384f9d261 249 * case 'm': //mでpの係数に0.01減算
shimizuta 32:297384f9d261 250 * P-=0.01;
shimizuta 32:297384f9d261 251 * break;
shimizuta 32:297384f9d261 252 * case 'j': //jでdの係数に0.01加算
shimizuta 32:297384f9d261 253 * D+=0.01;
shimizuta 32:297384f9d261 254 * break;
shimizuta 32:297384f9d261 255 * case 'n': //nでdの係数に0.01減算
shimizuta 32:297384f9d261 256 * D-=0.01;
shimizuta 32:297384f9d261 257 * break;
shimizuta 32:297384f9d261 258 * case 's':
shimizuta 32:297384f9d261 259 * motor.stop(); //sでモーター停止
shimizuta 32:297384f9d261 260 * target_omega=0;
shimizuta 32:297384f9d261 261 * break;
shimizuta 32:297384f9d261 262 * case 'x':
shimizuta 32:297384f9d261 263 * target_omega=TARGET_OMEGA; //xで速度制御開始
shimizuta 32:297384f9d261 264 * break;
shimizuta 32:297384f9d261 265 * default:
shimizuta 32:297384f9d261 266 * break;
shimizuta 32:297384f9d261 267 * }
shimizuta 32:297384f9d261 268 *
shimizuta 32:297384f9d261 269 * if(sel=='q'){
shimizuta 32:297384f9d261 270 * break; //qを押したら終了
shimizuta 32:297384f9d261 271 * }
shimizuta 32:297384f9d261 272 *
shimizuta 32:297384f9d261 273 * motor.setPDparam(P,D); //変更したパラメータをセット
shimizuta 32:297384f9d261 274 * }
shimizuta 32:297384f9d261 275 * kai++;
shimizuta 32:297384f9d261 276 * }
shimizuta 32:297384f9d261 277 * motor1.stop();
shimizuta 32:297384f9d261 278 * }
shimizuta 32:297384f9d261 279 * @endcode
shimizuta 32:297384f9d261 280 *
shimizuta 32:297384f9d261 281 */
shimizuta 32:297384f9d261 282 class SpeedControl : public Ec
shimizuta 32:297384f9d261 283 {
shimizuta 32:297384f9d261 284 private:
shimizuta 32:297384f9d261 285 double Kv_p,Kv_d;
shimizuta 32:297384f9d261 286 double diff,diff_old;
shimizuta 32:297384f9d261 287 double out_duty,out;
shimizuta 32:297384f9d261 288 double now_omega,now_RPM;
shimizuta 32:297384f9d261 289 double now_time_,old_time_;
shimizuta 32:297384f9d261 290 //Serial pc(USBTX,USBRX);
shimizuta 32:297384f9d261 291 protected:
shimizuta 32:297384f9d261 292
shimizuta 32:297384f9d261 293 public:
shimizuta 32:297384f9d261 294 PwmOut pwm_F_;
shimizuta 32:297384f9d261 295 PwmOut pwm_B_;
shimizuta 32:297384f9d261 296
shimizuta 32:297384f9d261 297 /** constractorの定義
shimizuta 32:297384f9d261 298 * @param signalA エンコーダA相のピン名
shimizuta 32:297384f9d261 299 * @param signalB エンコーダB相のピン名
shimizuta 32:297384f9d261 300 * @param signalZ エンコーダZ相のピン名
shimizuta 32:297384f9d261 301 * @param s エンコーダの分解能
shimizuta 32:297384f9d261 302 * @param t 角速度計算の間隔
shimizuta 32:297384f9d261 303 * @param pwm_F motorを正転させるpwmピン名
shimizuta 32:297384f9d261 304 * @param pwm_B motorを後転させるpwmピン名
shimizuta 32:297384f9d261 305 * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
shimizuta 32:297384f9d261 306 */
shimizuta 32:297384f9d261 307 SpeedControl( PinName signalA , PinName signalB , PinName signalZ , int s, double t, PinName pwm_F , PinName pwm_B);
shimizuta 32:297384f9d261 308
shimizuta 32:297384f9d261 309 /** duty比を角速度に変換させるための定数
shimizuta 32:297384f9d261 310 *
shimizuta 32:297384f9d261 311 * 使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める
shimizuta 32:297384f9d261 312 *
shimizuta 32:297384f9d261 313 * 多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない
shimizuta 32:297384f9d261 314 *
shimizuta 32:297384f9d261 315 * デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果
shimizuta 32:297384f9d261 316 *
shimizuta 32:297384f9d261 317 *
shimizuta 32:297384f9d261 318 */
shimizuta 32:297384f9d261 319 double C;
shimizuta 32:297384f9d261 320 /** 速度制御関数、引数は目標速度
shimizuta 32:297384f9d261 321 *
shimizuta 32:297384f9d261 322 * この目標角加速度になるようにモーターを回転させることができる
shimizuta 32:297384f9d261 323 *
shimizuta 32:297384f9d261 324 * 負の数を代入すれば逆回転することができる
shimizuta 32:297384f9d261 325 *
shimizuta 32:297384f9d261 326 * 出力できるduty比は最大で0.5までに設定してある
shimizuta 32:297384f9d261 327 * @param target_omega 目標角速度
shimizuta 32:297384f9d261 328 *
shimizuta 32:297384f9d261 329 * @section CAUTION（printfについて）
shimizuta 32:297384f9d261 330 *
shimizuta 32:297384f9d261 331 * 上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
shimizuta 32:297384f9d261 332 *
shimizuta 32:297384f9d261 333 * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
shimizuta 32:297384f9d261 334 *
shimizuta 32:297384f9d261 335 * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
shimizuta 32:297384f9d261 336 *
shimizuta 32:297384f9d261 337 * （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
shimizuta 32:297384f9d261 338 *
shimizuta 32:297384f9d261 339 */
shimizuta 32:297384f9d261 340 void Sc(double target_omega);
shimizuta 32:297384f9d261 341 /** PIDパラメータ設定関数
shimizuta 32:297384f9d261 342 *
shimizuta 32:297384f9d261 343 * 引数はそれぞれp,dパラメータ
shimizuta 32:297384f9d261 344 *
shimizuta 32:297384f9d261 345 * デフォルトは全部0に設定してある
shimizuta 32:297384f9d261 346 *
shimizuta 32:297384f9d261 347 * パラメーター値は実験的に頑張って求めるしかないです
shimizuta 32:297384f9d261 348 * @param p p制御パラメータ
shimizuta 32:297384f9d261 349 * @param d d制御パラメータ
shimizuta 32:297384f9d261 350 */
shimizuta 32:297384f9d261 351 void setPDparam(double p,double d);
shimizuta 32:297384f9d261 352
shimizuta 32:297384f9d261 353 /** 角速度・duty比変換定数(C)の設定関数
shimizuta 32:297384f9d261 354 *
shimizuta 32:297384f9d261 355 * 文字通りである
shimizuta 32:297384f9d261 356 * @param c duty比を角速度に変換させるための定数
shimizuta 32:297384f9d261 357 */
shimizuta 32:297384f9d261 358 void setDOconstant(double c);
shimizuta 32:297384f9d261 359
shimizuta 32:297384f9d261 360 /** モーター停止用関数 */
shimizuta 32:297384f9d261 361 void stop();
shimizuta 32:297384f9d261 362
shimizuta 32:297384f9d261 363 /** モーター正回転用関数
shimizuta 32:297384f9d261 364 @param duty 回転duty比
shimizuta 32:297384f9d261 365 */
shimizuta 32:297384f9d261 366 void turnF(double duty);
shimizuta 32:297384f9d261 367
shimizuta 32:297384f9d261 368 /** モーター逆回転用関数
shimizuta 32:297384f9d261 369 @param duty 回転duty比
shimizuta 32:297384f9d261 370 */
shimizuta 32:297384f9d261 371 void turnB(double duty);
shimizuta 32:297384f9d261 372
shimizuta 32:297384f9d261 373 /** 出力duty比
shimizuta 32:297384f9d261 374 */
shimizuta 32:297384f9d261 375 double duty;
shimizuta 32:297384f9d261 376
shimizuta 32:297384f9d261 377 /** Z相を用いた速度制御
shimizuta 32:297384f9d261 378 @param target_RPM 目標RPM
shimizuta 32:297384f9d261 379 */
shimizuta 32:297384f9d261 380 void ScZ2(double target_RPM);
shimizuta 32:297384f9d261 381
shimizuta 32:297384f9d261 382 //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
shimizuta 32:297384f9d261 383 //void ScZ(double target_RPM);
shimizuta 32:297384f9d261 384 //void Accelarate(double target_duty);
shimizuta 32:297384f9d261 385
shimizuta 32:297384f9d261 386 virtual void reset();
shimizuta 32:297384f9d261 387 };
shimizuta 32:297384f9d261 388 #endif