riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

EC.h@31:05e37fea072b, 2017-08-17 (annotated)

Committer:
jack0325suzu
Date:
Thu Aug 17 07:49:21 2017 +0000
Revision:
31:05e37fea072b
Parent:
30:087ad7703445
Child:
32:297384f9d261
???????????????

Who changed what in which revision?

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