7th_DENSOU / Mbed 2 deprecated nhk_2021motorCD_

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed nhk_2021motorCD ros_lib_melodic

EC/EC.h@2:342661f2a3f8, 2021-08-06 (annotated)

Committer:
koheim
Date:
Fri Aug 06 09:01:16 2021 +0000
Revision:
2:342661f2a3f8
Parent:
0:1d2320c10c69 
a

Who changed what in which revision?

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