7th_DENSOU / Mbed 2 deprecated nhk_2021motorAB_

motorAB

Dependencies:   mbed ros_lib_melodic

EC/EC.h@1:163629043391, 2021-05-15 (annotated)

Committer:
koheim
Date:
Sat May 15 08:34:17 2021 +0000
Revision:
1:163629043391
Parent:
0:cca61e773cbb 
nhk_motor(AB)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:cca61e773cbb 1 #ifndef __INCLUDED_EC_H_
la00noix 0:cca61e773cbb 2 #define __INCLUDED_EC_H_
la00noix 0:cca61e773cbb 3
la00noix 0:cca61e773cbb 4 #ifndef M_pi
la00noix 0:cca61e773cbb 5 #define M_pi 3.141592
la00noix 0:cca61e773cbb 6 #endif
la00noix 0:cca61e773cbb 7 /**increment型encoder用class
la00noix 0:cca61e773cbb 8
la00noix 0:cca61e773cbb 9 Z相(1周につき1回立ち上がる)の機能を追加しました!!!!
la00noix 0:cca61e773cbb 10
la00noix 0:cca61e773cbb 11 普通に使う分には不必要な機能ですが、回転回数が欲しい場合や、回転が非常に早い物の回転速度が欲しい場合などでA,B相での処理だとマイコンが追いつかない場合などに使ってください
la00noix 0:cca61e773cbb 12 */
la00noix 0:cca61e773cbb 13
la00noix 0:cca61e773cbb 14 class Ec{
la00noix 0:cca61e773cbb 15 protected:
la00noix 0:cca61e773cbb 16 int S; //A相B相のパターン（1~4）
la00noix 0:cca61e773cbb 17 bool stateA,stateB; //A・B相の状態
la00noix 0:cca61e773cbb 18 int count; //カウント数 分解能分で一周
la00noix 0:cca61e773cbb 19 double pre_count; //一つ前のカウント
la00noix 0:cca61e773cbb 20 double precount; //4倍精度カウント
la00noix 0:cca61e773cbb 21 int solution; //分解能
la00noix 0:cca61e773cbb 22 double dt; //角速度の計算間隔
la00noix 0:cca61e773cbb 23 double C_ec;
la00noix 0:cca61e773cbb 24
la00noix 0:cca61e773cbb 25 //z相用
la00noix 0:cca61e773cbb 26 bool first;
la00noix 0:cca61e773cbb 27 int rev;
la00noix 0:cca61e773cbb 28 double now_time,old_time;
la00noix 0:cca61e773cbb 29 double RPM,RPM_old;
la00noix 0:cca61e773cbb 30 int RPM_th;
la00noix 0:cca61e773cbb 31
la00noix 0:cca61e773cbb 32 InterruptIn signalA_;
la00noix 0:cca61e773cbb 33 InterruptIn signalB_;
la00noix 0:cca61e773cbb 34 InterruptIn signalZ_;
la00noix 0:cca61e773cbb 35
la00noix 0:cca61e773cbb 36 void upA();
la00noix 0:cca61e773cbb 37 void downA();
la00noix 0:cca61e773cbb 38 void upB();
la00noix 0:cca61e773cbb 39 void downB();
la00noix 0:cca61e773cbb 40 void upZ();
la00noix 0:cca61e773cbb 41 void downZ();
la00noix 0:cca61e773cbb 42
la00noix 0:cca61e773cbb 43 public:
la00noix 0:cca61e773cbb 44 double omega; //角速度
la00noix 0:cca61e773cbb 45 /** コンストラクタの定義
la00noix 0:cca61e773cbb 46 *
la00noix 0:cca61e773cbb 47 * ***Z相の機能を追加したことで引数が増えました!!!!***
la00noix 0:cca61e773cbb 48 *
la00noix 0:cca61e773cbb 49 * main関数の前に必ず一度宣言する
la00noix 0:cca61e773cbb 50 *
la00noix 0:cca61e773cbb 51 * 使うエンコーダの数だけ設定する必要がある
la00noix 0:cca61e773cbb 52 *
la00noix 0:cca61e773cbb 53 * @param signalA エンコーダA相のピン名
la00noix 0:cca61e773cbb 54 * @param signalB エンコーダB相のピン名
la00noix 0:cca61e773cbb 55 * @param signalZ エンコーダZ相のピン名
la00noix 0:cca61e773cbb 56 * @param s エンコーダの分解能(省略可)
la00noix 0:cca61e773cbb 57 * @param t 角速度計算の間隔(省略可)
la00noix 0:cca61e773cbb 58 */
la00noix 0:cca61e773cbb 59 /** @section CAUTION
la00noix 0:cca61e773cbb 60 * 今まで以下のように定義していたものは
la00noix 0:cca61e773cbb 61 * @code
la00noix 0:cca61e773cbb 62 * #include "mbed.h"
la00noix 0:cca61e773cbb 63 * #include "EC.h"
la00noix 0:cca61e773cbb 64 *
la00noix 0:cca61e773cbb 65 * Ec Ec1(PA_0,PA_1,1024,0.05);
la00noix 0:cca61e773cbb 66 * @endcode
la00noix 0:cca61e773cbb 67 * 次のようにZ相の引数の部分に、NCと入れれば今までの様に使える
la00noix 0:cca61e773cbb 68 * @code
la00noix 0:cca61e773cbb 69 * #include "mbed.h"
la00noix 0:cca61e773cbb 70 * #include "EC.h"
la00noix 0:cca61e773cbb 71 *
la00noix 0:cca61e773cbb 72 * Ec Ec1(PA_0,PA_1,NC,1024,0.05);
la00noix 0:cca61e773cbb 73 * @endcode
la00noix 0:cca61e773cbb 74 */
la00noix 0:cca61e773cbb 75 Ec(PinName signalA,PinName signalB,PinName signalZ,int s,double t);
la00noix 0:cca61e773cbb 76
la00noix 0:cca61e773cbb 77 ///countの値を返す関数(int型)
la00noix 0:cca61e773cbb 78 int getCount();
la00noix 0:cca61e773cbb 79 ///omega(角速度 rad/s)の値を計算する関数
la00noix 0:cca61e773cbb 80 /** @section CAUTION
la00noix 0:cca61e773cbb 81 * CalOmega関数は、一定時間ごと（dtごと）に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。
la00noix 0:cca61e773cbb 82 * @code
la00noix 0:cca61e773cbb 83 * #include "mbed.h"
la00noix 0:cca61e773cbb 84 * #include "EC.h" //ライブラリをインクルード
la00noix 0:cca61e773cbb 85 *
la00noix 0:cca61e773cbb 86 * Ec Ec1(PA_0,PA_1,NC,1024,0.05); //分解能1024、計算間隔0.05秒に設定、Z相は使わない
la00noix 0:cca61e773cbb 87 * Ticker ticker;
la00noix 0:cca61e773cbb 88 * DigitalIn button(USER_BUTTON);
la00noix 0:cca61e773cbb 89 * Serial pc(USBTX,USBRX);
la00noix 0:cca61e773cbb 90 *
la00noix 0:cca61e773cbb 91 * void calOmega(){
la00noix 0:cca61e773cbb 92 * Ec1.CalOmega();
la00noix 0:cca61e773cbb 93 * }
la00noix 0:cca61e773cbb 94 *
la00noix 0:cca61e773cbb 95 * int main(){
la00noix 0:cca61e773cbb 96 * ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
la00noix 0:cca61e773cbb 97 * while(1){
la00noix 0:cca61e773cbb 98 * pc.printf(" count1=%d ",Ec1.getCount());
la00noix 0:cca61e773cbb 99 * pc.printf(" omega1=%f\r\n ",Ec1.getOmega());
la00noix 0:cca61e773cbb 100 * if(pc.readable()) {
la00noix 0:cca61e773cbb 101 * char sel=pc.getc();
la00noix 0:cca61e773cbb 102 * if(sel=='r') Ec1.reset(); //rを押したらリセット
la00noix 0:cca61e773cbb 103 * }
la00noix 0:cca61e773cbb 104 * }
la00noix 0:cca61e773cbb 105 * }
la00noix 0:cca61e773cbb 106 * @endcode
la00noix 0:cca61e773cbb 107 */
la00noix 0:cca61e773cbb 108 void CalOmega();
la00noix 0:cca61e773cbb 109 ///omega(角速度 rad/s)の値を返す関数(double型)
la00noix 0:cca61e773cbb 110 double getOmega();
la00noix 0:cca61e773cbb 111 ///四倍精度のcountの値を返す関数(double型)
la00noix 0:cca61e773cbb 112 double getPreCount();
la00noix 0:cca61e773cbb 113 ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする
la00noix 0:cca61e773cbb 114 //virtual void reset();
la00noix 0:cca61e773cbb 115 ///角速度計算の間隔dtを決めることができる（デフォルトは0.05秒）
la00noix 0:cca61e773cbb 116 void setTime(double t);
la00noix 0:cca61e773cbb 117 ///(Z相を使用する場合)回転回数を返す関数(int型)
la00noix 0:cca61e773cbb 118 int getRev();
la00noix 0:cca61e773cbb 119 ///(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型)
la00noix 0:cca61e773cbb 120 /** @section SAMPLE
la00noix 0:cca61e773cbb 121 * z相を使う場合のプログラムの例
la00noix 0:cca61e773cbb 122 * @code
la00noix 0:cca61e773cbb 123 * #include "mbed.h"
la00noix 0:cca61e773cbb 124 * #include "EC.h" //ライブラリをインクルード
la00noix 0:cca61e773cbb 125 *
la00noix 0:cca61e773cbb 126 * Ec Ec1(NC,NC,PA_0,1024,0.05); //A相B相が不必要な場合も、このようにNCと入れればよい
la00noix 0:cca61e773cbb 127 * DigitalIn button(USER_BUTTON);
la00noix 0:cca61e773cbb 128 * Serial pc(USBTX,USBRX);
la00noix 0:cca61e773cbb 129 *
la00noix 0:cca61e773cbb 130 * int main(){
la00noix 0:cca61e773cbb 131 * while(1){
la00noix 0:cca61e773cbb 132 * pc.printf(" rev1=%d ",Ec1.getRev());
la00noix 0:cca61e773cbb 133 * pc.printf(" rpm1=%f\r\n ",Ec1.getRPM());
la00noix 0:cca61e773cbb 134 * if(!button) Ec1.reset(); //USERボタンを押したらリセット
la00noix 0:cca61e773cbb 135 * }
la00noix 0:cca61e773cbb 136 * }
la00noix 0:cca61e773cbb 137 * @endcode
la00noix 0:cca61e773cbb 138 */
la00noix 0:cca61e773cbb 139 double getRPM();
la00noix 0:cca61e773cbb 140
la00noix 0:cca61e773cbb 141 void changeRPM_th(int th);
la00noix 0:cca61e773cbb 142
la00noix 0:cca61e773cbb 143
la00noix 0:cca61e773cbb 144 ///角速度計算の間隔
la00noix 0:cca61e773cbb 145 static double deftime;
la00noix 0:cca61e773cbb 146 ///エンコーダの分解能のデフォルト値
la00noix 0:cca61e773cbb 147 static int defsolution;
la00noix 0:cca61e773cbb 148
la00noix 0:cca61e773cbb 149 Timer timer;
la00noix 0:cca61e773cbb 150
la00noix 0:cca61e773cbb 151 };
la00noix 0:cca61e773cbb 152
la00noix 0:cca61e773cbb 153
la00noix 0:cca61e773cbb 154 /**速度制御用ライブラリ
la00noix 0:cca61e773cbb 155 *
la00noix 0:cca61e773cbb 156 * サンプルプログラム
la00noix 0:cca61e773cbb 157 * @code
la00noix 0:cca61e773cbb 158 * //サンプルプログラム 差動二輪駆動ロボットを想定
la00noix 0:cca61e773cbb 159 * //直進・後退・右旋回・左旋回が可能
la00noix 0:cca61e773cbb 160 * #include "mbed.h"
la00noix 0:cca61e773cbb 161 * #include "EC.h"
la00noix 0:cca61e773cbb 162 *
la00noix 0:cca61e773cbb 163 * #define BASIC_SPEED 20 //モーターはこの角速度で駆動させる
la00noix 0:cca61e773cbb 164 *
la00noix 0:cca61e773cbb 165 * SpeedControl motorR(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //右輪
la00noix 0:cca61e773cbb 166 * SpeedControl motorL(PF_1,PA_11,NC,1048,0.05,PA_9,PA_10); //左輪
la00noix 0:cca61e773cbb 167 * Ticker motor_tick; //角速度計算用ticker
la00noix 0:cca61e773cbb 168 * Serial pc(USBTX,USBRX);
la00noix 0:cca61e773cbb 169 *
la00noix 0:cca61e773cbb 170 * void calOmega() //角速度計算関数
la00noix 0:cca61e773cbb 171 * {
la00noix 0:cca61e773cbb 172 * motorR.CalOmega();
la00noix 0:cca61e773cbb 173 * motorL.CalOmega();
la00noix 0:cca61e773cbb 174 * }
la00noix 0:cca61e773cbb 175 *
la00noix 0:cca61e773cbb 176 * int main(void){
la00noix 0:cca61e773cbb 177 * motor_tick.attach(&calOmega,0.05);
la00noix 0:cca61e773cbb 178 * //Cの値を設定
la00noix 0:cca61e773cbb 179 * motorR.setDOconstant(46.3);
la00noix 0:cca61e773cbb 180 * motorL.setDOconstant(47.2);
la00noix 0:cca61e773cbb 181 * //pd係数を設定
la00noix 0:cca61e773cbb 182 * motorR.setPDparam(0.1,0);
la00noix 0:cca61e773cbb 183 * motorR.setPDparam(0.12,0);
la00noix 0:cca61e773cbb 184 *
la00noix 0:cca61e773cbb 185 * int kai=0;
la00noix 0:cca61e773cbb 186 * double target_R=0,target_L=0;
la00noix 0:cca61e773cbb 187 * char status[5][10]={"STOP","FORWARD","BACKWARD","RIGHT","LEFT"};
la00noix 0:cca61e773cbb 188 * int status_num=0; //初期状態は停止
la00noix 0:cca61e773cbb 189 *
la00noix 0:cca61e773cbb 190 * while(1) {
la00noix 0:cca61e773cbb 191 * motorR.Sc(target_R);
la00noix 0:cca61e773cbb 192 * motorL.Sc(target_L); //target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
la00noix 0:cca61e773cbb 193 *
la00noix 0:cca61e773cbb 194 * if(kai>=500) {
la00noix 0:cca61e773cbb 195 * pc.printf("status=%s omega_R=%f omega_L=%f \r\n",status[status_num],motorR.getOmega(),motorL.getOmega());
la00noix 0:cca61e773cbb 196 * //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
la00noix 0:cca61e773cbb 197 * kai=0;
la00noix 0:cca61e773cbb 198 * }
la00noix 0:cca61e773cbb 199 *
la00noix 0:cca61e773cbb 200 * if(pc.readable()) {
la00noix 0:cca61e773cbb 201 * char sel=pc.getc();
la00noix 0:cca61e773cbb 202 *
la00noix 0:cca61e773cbb 203 * switch (sel) {
la00noix 0:cca61e773cbb 204 * case 's': //停止
la00noix 0:cca61e773cbb 205 * motorR.stop();
la00noix 0:cca61e773cbb 206 * motorL.stop();
la00noix 0:cca61e773cbb 207 * target_R=0;
la00noix 0:cca61e773cbb 208 * target_L=0;
la00noix 0:cca61e773cbb 209 * status_num=0;
la00noix 0:cca61e773cbb 210 * break;
la00noix 0:cca61e773cbb 211 * case 'i': //前進
la00noix 0:cca61e773cbb 212 * target_R=BASIC_SPEED;
la00noix 0:cca61e773cbb 213 * target_L=BASIC_SPEED;
la00noix 0:cca61e773cbb 214 * status_num=1;
la00noix 0:cca61e773cbb 215 * break;
la00noix 0:cca61e773cbb 216 * case 'm': //後退
la00noix 0:cca61e773cbb 217 * target_R=(-1)*BASIC_SPEED;
la00noix 0:cca61e773cbb 218 * target_L=(-1)*BASIC_SPEED;
la00noix 0:cca61e773cbb 219 * status_num=2;
la00noix 0:cca61e773cbb 220 * break;
la00noix 0:cca61e773cbb 221 * case 'k': //右旋回
la00noix 0:cca61e773cbb 222 * motorR.stop();
la00noix 0:cca61e773cbb 223 * target_R=0;
la00noix 0:cca61e773cbb 224 * target_L=BASIC_SPEED;
la00noix 0:cca61e773cbb 225 * status_num=3;
la00noix 0:cca61e773cbb 226 * break;
la00noix 0:cca61e773cbb 227 * case 'j': //左旋回
la00noix 0:cca61e773cbb 228 * motorL.stop();
la00noix 0:cca61e773cbb 229 * target_R=BASIC_SPEED;
la00noix 0:cca61e773cbb 230 * target_L=0;
la00noix 0:cca61e773cbb 231 * status_num=4;
la00noix 0:cca61e773cbb 232 * break;
la00noix 0:cca61e773cbb 233 * default:
la00noix 0:cca61e773cbb 234 * break;
la00noix 0:cca61e773cbb 235 * }
la00noix 0:cca61e773cbb 236 * if(sel=='q'){
la00noix 0:cca61e773cbb 237 * break; //qを押したら左右のモーターを停止
la00noix 0:cca61e773cbb 238 * }
la00noix 0:cca61e773cbb 239 * }
la00noix 0:cca61e773cbb 240 * kai++;
la00noix 0:cca61e773cbb 241 * }
la00noix 0:cca61e773cbb 242 * motorR.stop();
la00noix 0:cca61e773cbb 243 * motorL.stop();
la00noix 0:cca61e773cbb 244 *
la00noix 0:cca61e773cbb 245 * return 0;
la00noix 0:cca61e773cbb 246 * }
la00noix 0:cca61e773cbb 247 * @endcode
la00noix 0:cca61e773cbb 248 * @section 基本的な速度制御の実用までの流れ
la00noix 0:cca61e773cbb 249 *
la00noix 0:cca61e773cbb 250 * 1. モーター・エンコーダ・モータードライバ・マイコン・バッテリーを適切につなげる
la00noix 0:cca61e773cbb 251 *
la00noix 0:cca61e773cbb 252 * 接続のイメージ↓
la00noix 0:cca61e773cbb 253 *
la00noix 0:cca61e773cbb 254 * https://www.fastpic.jp/images.php?file=0391548523.png
la00noix 0:cca61e773cbb 255 *
la00noix 0:cca61e773cbb 256 * 注意！！！ リポ用のスイッチと電圧計を必ずつけること！！！
la00noix 0:cca61e773cbb 257 *
la00noix 0:cca61e773cbb 258 * /
la00noix 0:cca61e773cbb 259 *
la00noix 0:cca61e773cbb 260 * 2. 実際にモーターを回転させてみて不備なく動作するか確認する
la00noix 0:cca61e773cbb 261 *
la00noix 0:cca61e773cbb 262 * 確認事項
la00noix 0:cca61e773cbb 263 *
la00noix 0:cca61e773cbb 264 * ・モーターが回るか、duty比によって加速・減速するか
la00noix 0:cca61e773cbb 265 *
la00noix 0:cca61e773cbb 266 * ・エンコーダの値が取れているか
la00noix 0:cca61e773cbb 267 *
la00noix 0:cca61e773cbb 268 * ・モーターの回転方向とエンコーダの加算減算が対応しているか (プログラム内で定義した正・負回転とエンコーダの増減が対応しているかということ)
la00noix 0:cca61e773cbb 269 *
la00noix 0:cca61e773cbb 270 * などなど。コードは下の一つ目のものを使えばいい
la00noix 0:cca61e773cbb 271 *
la00noix 0:cca61e773cbb 272 * /
la00noix 0:cca61e773cbb 273 *
la00noix 0:cca61e773cbb 274 * 3. Cの値を見つける
la00noix 0:cca61e773cbb 275 *
la00noix 0:cca61e773cbb 276 * このライブラリでは、「モーターにかかる電圧（duty比）と回転速度はある程度比例の関係にあるだろう」という考え方をしている（もちろん実際のモデルはもっと複雑である）。
la00noix 0:cca61e773cbb 277 *
la00noix 0:cca61e773cbb 278 * このライブラリにおける速度制御のアルゴリズムとしては、あらかじめ求めたこの比例係数を用いて、目標角速度からだいたいのduty比を算出し、そこからpd制御をかけるという形をとっている。
la00noix 0:cca61e773cbb 279 *
la00noix 0:cca61e773cbb 280 * Cとはこの比例係数のことである。一応式としてあらわすと、
la00noix 0:cca61e773cbb 281 *
la00noix 0:cca61e773cbb 282 * （回転速度）＝C×（duty比）
la00noix 0:cca61e773cbb 283 *
la00noix 0:cca61e773cbb 284 * となる。このCの値はモーターやギアユニットによって固有の値であり、それぞれのモーターにおいて求める必要がある。
la00noix 0:cca61e773cbb 285 *
la00noix 0:cca61e773cbb 286 * 具体的なこの係数の求め方は、duty比を0.05くらいずつで変化させ、その時のduty比と回転角速度を記録し、excelからグラフの傾きを求めればよい。
la00noix 0:cca61e773cbb 287 *
la00noix 0:cca61e773cbb 288 * こんな感じになるよっていう画像↓
la00noix 0:cca61e773cbb 289 *
la00noix 0:cca61e773cbb 290 * https://www.fastpic.jp/images.php?file=2416798781.gif
la00noix 0:cca61e773cbb 291 *
la00noix 0:cca61e773cbb 292 * コードは下の一つ目のものを使えばいい
la00noix 0:cca61e773cbb 293 *
la00noix 0:cca61e773cbb 294 * /
la00noix 0:cca61e773cbb 295 *
la00noix 0:cca61e773cbb 296 * 4. pd係数を見つける
la00noix 0:cca61e773cbb 297 *
la00noix 0:cca61e773cbb 298 * 上記の通り、最終的にはpd制御（精確にはi+pd制御）によって目標角速度に収束させる。
la00noix 0:cca61e773cbb 299 *
la00noix 0:cca61e773cbb 300 * ここではそのpd制御の操作量を決定する係数を見つける。これも実験的に求めるほかなく、値を変えて挙動を見てを繰り返して最適な係数を見つける。
la00noix 0:cca61e773cbb 301 *
la00noix 0:cca61e773cbb 302 * 値の評価は、目標に達するまでの速さ、オーバーシュートの少なさ、収束後の値の振動の小ささなどである。
la00noix 0:cca61e773cbb 303 *
la00noix 0:cca61e773cbb 304 * 重要度的にはp制御＞d制御（場合によってはd制御が不必要なことも）なので、まずdの係数を0にした状態で先にpの係数を見つける。
la00noix 0:cca61e773cbb 305 *
la00noix 0:cca61e773cbb 306 * コードは下の二つ目のものを使えばいい
la00noix 0:cca61e773cbb 307 *
la00noix 0:cca61e773cbb 308 * @code
la00noix 0:cca61e773cbb 309 * //2,3で使えるサンプルコード
la00noix 0:cca61e773cbb 310 * //入力はTera Termで行う
la00noix 0:cca61e773cbb 311 * #include "mbed.h"
la00noix 0:cca61e773cbb 312 * #include "EC.h"
la00noix 0:cca61e773cbb 313 *
la00noix 0:cca61e773cbb 314 * SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5);
la00noix 0:cca61e773cbb 315 * Serial pc(USBTX,USBRX);
la00noix 0:cca61e773cbb 316 * Ticker motor_tick;
la00noix 0:cca61e773cbb 317 *
la00noix 0:cca61e773cbb 318 * void calOmega(){
la00noix 0:cca61e773cbb 319 * motor.CalOmega(); //角速度計算用
la00noix 0:cca61e773cbb 320 * }
la00noix 0:cca61e773cbb 321 *
la00noix 0:cca61e773cbb 322 * int main(void){
la00noix 0:cca61e773cbb 323 * motor_tick.attach(calOmega,0.05);
la00noix 0:cca61e773cbb 324 * int loop_time=0;
la00noix 0:cca61e773cbb 325 * float in_duty=0;
la00noix 0:cca61e773cbb 326 * bool print=false;
la00noix 0:cca61e773cbb 327 *
la00noix 0:cca61e773cbb 328 * while(1){
la00noix 0:cca61e773cbb 329 * loop_time++;
la00noix 0:cca61e773cbb 330 *
la00noix 0:cca61e773cbb 331 * if(in_duty>0) motor.turnF(in_duty); //in_dutyが正のとき正回転（このプログラムではPA_8で駆動）
la00noix 0:cca61e773cbb 332 * else motor.turnB((-1)*in_duty); //in_dutyが負のとき逆回転（このプログラムではPB_5で駆動）
la00noix 0:cca61e773cbb 333 *
la00noix 0:cca61e773cbb 334 *
la00noix 0:cca61e773cbb 335 * if(pc.readable()){
la00noix 0:cca61e773cbb 336 * char sel=pc.getc();
la00noix 0:cca61e773cbb 337 * if(sel=='s'){
la00noix 0:cca61e773cbb 338 * motor.stop();
la00noix 0:cca61e773cbb 339 * } else if(sel=='f'){
la00noix 0:cca61e773cbb 340 * in_duty+=(float)0.05; //fを押すとduty比がo.o5ずつあがる
la00noix 0:cca61e773cbb 341 * pc.printf("duty=%f\r\n",in_duty);
la00noix 0:cca61e773cbb 342 * } else if(sel=='b'){
la00noix 0:cca61e773cbb 343 * in_duty-=(float)0.05; //bを押すとduty比がo.o5ずつさがる
la00noix 0:cca61e773cbb 344 * pc.printf("duty=%f\r\n",in_duty);
la00noix 0:cca61e773cbb 345 * } else if(sel=='p'){
la00noix 0:cca61e773cbb 346 * print=!print; //pを押すと表示を停止/再開する
la00noix 0:cca61e773cbb 347 * }
la00noix 0:cca61e773cbb 348 * }
la00noix 0:cca61e773cbb 349 *
la00noix 0:cca61e773cbb 350 * if(loop_time%1000==0){
la00noix 0:cca61e773cbb 351 * 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_);
la00noix 0:cca61e773cbb 352 * } //左からエンコーダのカウント、角速度、正方向出力duty比、負方向出力duty比を表示
la00noix 0:cca61e773cbb 353 * }
la00noix 0:cca61e773cbb 354 * }
la00noix 0:cca61e773cbb 355 * @endcode
la00noix 0:cca61e773cbb 356 *
la00noix 0:cca61e773cbb 357 * @code
la00noix 0:cca61e773cbb 358 * //PDの係数を探したいときに使えるサンプル
la00noix 0:cca61e773cbb 359 * //入力はTera Termで行う
la00noix 0:cca61e773cbb 360 * #include "mbed.h"
la00noix 0:cca61e773cbb 361 * #include "EC.h" //ヘッダファイルをインクルード
la00noix 0:cca61e773cbb 362 *
la00noix 0:cca61e773cbb 363 * #define TARGET_OMEGA 20 //目標角速度を20rad/sに設定
la00noix 0:cca61e773cbb 364 *
la00noix 0:cca61e773cbb 365 * SpeedControl motor(PF_0,PB_1,NC,1048,0.05,PA_8,PB_5); //エンコーダ分解能1024、角速度計算間隔を0.05秒に設定
la00noix 0:cca61e773cbb 366 * Ticker motor_tick; //角速度計算用ticker
la00noix 0:cca61e773cbb 367 * Serial pc(USBTX,USBRX);
la00noix 0:cca61e773cbb 368 *
la00noix 0:cca61e773cbb 369 * void calOmega() //角速度計算関数
la00noix 0:cca61e773cbb 370 * {
la00noix 0:cca61e773cbb 371 * motor.CalOmega();
la00noix 0:cca61e773cbb 372 * }
la00noix 0:cca61e773cbb 373 *
la00noix 0:cca61e773cbb 374 * int main()
la00noix 0:cca61e773cbb 375 * {
la00noix 0:cca61e773cbb 376 * motor_tick.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
la00noix 0:cca61e773cbb 377 * motor.setDOconstant(46.3); //求めたCの値を設定
la00noix 0:cca61e773cbb 378 *
la00noix 0:cca61e773cbb 379 * int kai=0;
la00noix 0:cca61e773cbb 380 * double target_omega=0;
la00noix 0:cca61e773cbb 381 * double P=0.0,D=0.0;
la00noix 0:cca61e773cbb 382 *
la00noix 0:cca61e773cbb 383 * while(1) {
la00noix 0:cca61e773cbb 384 * motor.Sc(target_omega); //速度制御のコア関数
la00noix 0:cca61e773cbb 385 * if(kai>=500) {
la00noix 0:cca61e773cbb 386 * 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);
la00noix 0:cca61e773cbb 387 * //エンコーダのカウント、角速度、目標角速度、正方向出力duty比、負方向出力duty比、Pの係数、Dの係数を表示
la00noix 0:cca61e773cbb 388 * kai=0;
la00noix 0:cca61e773cbb 389 * }
la00noix 0:cca61e773cbb 390 * //係数探しの流れとしては、係数変更→速度制御開始→収束具合を観察・比較→モーター停止の繰り返し
la00noix 0:cca61e773cbb 391 * if(pc.readable()) {
la00noix 0:cca61e773cbb 392 * char sel=pc.getc();
la00noix 0:cca61e773cbb 393 *
la00noix 0:cca61e773cbb 394 * switch (sel) {
la00noix 0:cca61e773cbb 395 * case 'k': //kでpの係数に0.01加算
la00noix 0:cca61e773cbb 396 * P+=0.01;
la00noix 0:cca61e773cbb 397 * break;
la00noix 0:cca61e773cbb 398 * case 'm': //mでpの係数に0.01減算
la00noix 0:cca61e773cbb 399 * P-=0.01;
la00noix 0:cca61e773cbb 400 * break;
la00noix 0:cca61e773cbb 401 * case 'j': //jでdの係数に0.01加算
la00noix 0:cca61e773cbb 402 * D+=0.01;
la00noix 0:cca61e773cbb 403 * break;
la00noix 0:cca61e773cbb 404 * case 'n': //nでdの係数に0.01減算
la00noix 0:cca61e773cbb 405 * D-=0.01;
la00noix 0:cca61e773cbb 406 * break;
la00noix 0:cca61e773cbb 407 * case 's':
la00noix 0:cca61e773cbb 408 * motor.stop(); //sでモーター停止
la00noix 0:cca61e773cbb 409 * target_omega=0;
la00noix 0:cca61e773cbb 410 * break;
la00noix 0:cca61e773cbb 411 * case 'x':
la00noix 0:cca61e773cbb 412 * target_omega=TARGET_OMEGA; //xで速度制御開始
la00noix 0:cca61e773cbb 413 * break;
la00noix 0:cca61e773cbb 414 * default:
la00noix 0:cca61e773cbb 415 * break;
la00noix 0:cca61e773cbb 416 * }
la00noix 0:cca61e773cbb 417 *
la00noix 0:cca61e773cbb 418 * if(sel=='q'){
la00noix 0:cca61e773cbb 419 * break; //qを押したら終了
la00noix 0:cca61e773cbb 420 * }
la00noix 0:cca61e773cbb 421 *
la00noix 0:cca61e773cbb 422 * motor.setPDparam(P,D); //変更したパラメータをセット
la00noix 0:cca61e773cbb 423 * }
la00noix 0:cca61e773cbb 424 * kai++;
la00noix 0:cca61e773cbb 425 * }
la00noix 0:cca61e773cbb 426 * motor1.stop();
la00noix 0:cca61e773cbb 427 * }
la00noix 0:cca61e773cbb 428 * @endcode
la00noix 0:cca61e773cbb 429 *
la00noix 0:cca61e773cbb 430 */
la00noix 0:cca61e773cbb 431
la00noix 0:cca61e773cbb 432
la00noix 0:cca61e773cbb 433 class SpeedControl : public Ec{
la00noix 0:cca61e773cbb 434 private:
la00noix 0:cca61e773cbb 435 double Kv_p,Kv_d;
la00noix 0:cca61e773cbb 436 double diff,diff_old;
la00noix 0:cca61e773cbb 437 double out_duty,out_plus,out_minus;
la00noix 0:cca61e773cbb 438 double now_omega,now_RPM;
la00noix 0:cca61e773cbb 439 double now_time_,old_time_;
la00noix 0:cca61e773cbb 440 //Serial pc(USBTX,USBRX);
la00noix 0:cca61e773cbb 441 protected:
la00noix 0:cca61e773cbb 442
la00noix 0:cca61e773cbb 443 public:
la00noix 0:cca61e773cbb 444 PwmOut pwm_F_;
la00noix 0:cca61e773cbb 445 PwmOut pwm_B_;
la00noix 0:cca61e773cbb 446
la00noix 0:cca61e773cbb 447 /** constractorの定義
la00noix 0:cca61e773cbb 448 * @param signalA エンコーダA相のピン名
la00noix 0:cca61e773cbb 449 * @param signalB エンコーダB相のピン名
la00noix 0:cca61e773cbb 450 * @param signalZ エンコーダZ相のピン名
la00noix 0:cca61e773cbb 451 * @param s エンコーダの分解能
la00noix 0:cca61e773cbb 452 * @param t 角速度計算の間隔
la00noix 0:cca61e773cbb 453 * @param pwm_F motorを正転させるpwmピン名
la00noix 0:cca61e773cbb 454 * @param pwm_B motorを後転させるpwmピン名
la00noix 0:cca61e773cbb 455 * モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
la00noix 0:cca61e773cbb 456 */
la00noix 0:cca61e773cbb 457 SpeedControl( PinName signalA , PinName signalB , PinName signalZ , int s, double t, PinName pwm_F , PinName pwm_B);
la00noix 0:cca61e773cbb 458
la00noix 0:cca61e773cbb 459 /** duty比を角速度に変換させるための定数
la00noix 0:cca61e773cbb 460 *
la00noix 0:cca61e773cbb 461 * 使うモーターやギア比や供給電圧によって変化するので一度duty比と角速度の関係を調べてグラフの傾きから求める
la00noix 0:cca61e773cbb 462 *
la00noix 0:cca61e773cbb 463 * 多少ずれてもPIDが何とかしてくれるのでそこまでの精度は必要ない
la00noix 0:cca61e773cbb 464 *
la00noix 0:cca61e773cbb 465 * デフォルトは45 これは380モータとエンコーダを直結して12V電圧で動かしたときの結果
la00noix 0:cca61e773cbb 466 *
la00noix 0:cca61e773cbb 467 *
la00noix 0:cca61e773cbb 468 */
la00noix 0:cca61e773cbb 469 double Cf,Df,Cb,Db;
la00noix 0:cca61e773cbb 470 /** 速度制御関数、引数は目標速度
la00noix 0:cca61e773cbb 471 *
la00noix 0:cca61e773cbb 472 * この目標角加速度になるようにモーターを回転させることができる
la00noix 0:cca61e773cbb 473 *
la00noix 0:cca61e773cbb 474 * 負の数を代入すれば逆回転することができる
la00noix 0:cca61e773cbb 475 *
la00noix 0:cca61e773cbb 476 * 出力できるduty比は最大で0.5までに設定してある
la00noix 0:cca61e773cbb 477 * @param target_omega 目標角速度
la00noix 0:cca61e773cbb 478 *
la00noix 0:cca61e773cbb 479 * @section CAUTION（printfについて）
la00noix 0:cca61e773cbb 480 *
la00noix 0:cca61e773cbb 481 * 上記のプログラムのように、速度制御ではループ500回ごとにprintfをしている。
la00noix 0:cca61e773cbb 482 *
la00noix 0:cca61e773cbb 483 * printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
la00noix 0:cca61e773cbb 484 *
la00noix 0:cca61e773cbb 485 * なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
la00noix 0:cca61e773cbb 486 *
la00noix 0:cca61e773cbb 487 * （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
la00noix 0:cca61e773cbb 488 *
la00noix 0:cca61e773cbb 489 */
la00noix 0:cca61e773cbb 490 void Sc(double target_omega);
la00noix 0:cca61e773cbb 491 /** PIDパラメータ設定関数
la00noix 0:cca61e773cbb 492 *
la00noix 0:cca61e773cbb 493 * 引数はそれぞれp,dパラメータ
la00noix 0:cca61e773cbb 494 *
la00noix 0:cca61e773cbb 495 * デフォルトは全部0に設定してある
la00noix 0:cca61e773cbb 496 *
la00noix 0:cca61e773cbb 497 * パラメーター値は実験的に頑張って求めるしかないです
la00noix 0:cca61e773cbb 498 * @param p p制御パラメータ
la00noix 0:cca61e773cbb 499 * @param d d制御パラメータ
la00noix 0:cca61e773cbb 500 */
la00noix 0:cca61e773cbb 501 void setPDparam(double p,double d);
la00noix 0:cca61e773cbb 502
la00noix 0:cca61e773cbb 503 /** 角速度・duty比変換定数(C)の設定関数
la00noix 0:cca61e773cbb 504 *
la00noix 0:cca61e773cbb 505 * 文字通りである
la00noix 0:cca61e773cbb 506 * @param c duty比を角速度に変換させるための定数
la00noix 0:cca61e773cbb 507 */
la00noix 0:cca61e773cbb 508 void setDOconstant(double cf,double df,double cb,double db);
la00noix 0:cca61e773cbb 509
la00noix 0:cca61e773cbb 510 /** モーター停止用関数 */
la00noix 0:cca61e773cbb 511 void stop();
la00noix 0:cca61e773cbb 512
la00noix 0:cca61e773cbb 513 /** モーター正回転用関数
la00noix 0:cca61e773cbb 514 @param duty 回転duty比
la00noix 0:cca61e773cbb 515 */
la00noix 0:cca61e773cbb 516 void turnF(double duty);
la00noix 0:cca61e773cbb 517
la00noix 0:cca61e773cbb 518 /** モーター逆回転用関数
la00noix 0:cca61e773cbb 519 @param duty 回転duty比
la00noix 0:cca61e773cbb 520 */
la00noix 0:cca61e773cbb 521 void turnB(double duty);
la00noix 0:cca61e773cbb 522
la00noix 0:cca61e773cbb 523 /** 出力duty比
la00noix 0:cca61e773cbb 524 */
la00noix 0:cca61e773cbb 525 double duty;
la00noix 0:cca61e773cbb 526 double sth_debug;
la00noix 0:cca61e773cbb 527
la00noix 0:cca61e773cbb 528 /** Z相を用いた速度制御
la00noix 0:cca61e773cbb 529 @param target_RPM 目標RPM
la00noix 0:cca61e773cbb 530 */
la00noix 0:cca61e773cbb 531 //void ScZ2(double target_RPM);
la00noix 0:cca61e773cbb 532
la00noix 0:cca61e773cbb 533 //RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
la00noix 0:cca61e773cbb 534 //void ScZ(double target_RPM);
la00noix 0:cca61e773cbb 535 //void Accelarate(double target_duty);
la00noix 0:cca61e773cbb 536
la00noix 0:cca61e773cbb 537 virtual void reset();
la00noix 0:cca61e773cbb 538 };
la00noix 0:cca61e773cbb 539 #endif