春ロボ2019 経路追従プログラム(ver.3)
Dependencies: CruizCore_R1370P
Fork of PathFollowing1-ver2 by
EC.h@2:cd72726e166f, 2018-11-01 (annotated)
- Committer:
- yuki0701
- Date:
- Thu Nov 01 03:03:45 2018 +0000
- Revision:
- 2:cd72726e166f
- Parent:
- 0:3d9101dd0061
a;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuki0701 | 0:3d9101dd0061 | 1 | #ifndef __INCLUDED_EC_H_ |
yuki0701 | 0:3d9101dd0061 | 2 | #define __INCLUDED_EC_H_ |
yuki0701 | 0:3d9101dd0061 | 3 | |
yuki0701 | 0:3d9101dd0061 | 4 | #ifndef M_pi |
yuki0701 | 0:3d9101dd0061 | 5 | #define M_pi 3.141592 |
yuki0701 | 0:3d9101dd0061 | 6 | #endif |
yuki0701 | 0:3d9101dd0061 | 7 | #include "mbed.h" |
yuki0701 | 0:3d9101dd0061 | 8 | /**@class Ec |
yuki0701 | 0:3d9101dd0061 | 9 | @brief increment型encoder用class. |
yuki0701 | 0:3d9101dd0061 | 10 | Z相(1周につき1回立ち上がる)の機能あり. |
yuki0701 | 0:3d9101dd0061 | 11 | 普通に使う分には不必要な機能ですが、回転回数が欲しい場合や、回転が非常に早い物の回転速度が欲しい場合などで |
yuki0701 | 0:3d9101dd0061 | 12 | A,B相での処理だとマイコンが追いつかない場合などに使ってください |
yuki0701 | 0:3d9101dd0061 | 13 | */ |
yuki0701 | 0:3d9101dd0061 | 14 | class Ec |
yuki0701 | 0:3d9101dd0061 | 15 | { |
yuki0701 | 0:3d9101dd0061 | 16 | protected: |
yuki0701 | 0:3d9101dd0061 | 17 | int S; //A相B相のパターン(1~4) |
yuki0701 | 0:3d9101dd0061 | 18 | bool stateA,stateB; //A・B相の状態 |
yuki0701 | 0:3d9101dd0061 | 19 | int count; //カウント数 分解能分で一周 |
yuki0701 | 0:3d9101dd0061 | 20 | int pre_count; //一つ前のカウント |
yuki0701 | 0:3d9101dd0061 | 21 | double precount; //4倍精度カウント |
yuki0701 | 0:3d9101dd0061 | 22 | int solution; //分解能 |
yuki0701 | 0:3d9101dd0061 | 23 | double dt; //角速度の計算間隔 |
yuki0701 | 0:3d9101dd0061 | 24 | double distance_mm_;//距離[mm] |
yuki0701 | 0:3d9101dd0061 | 25 | double diameter_mm_;//測定輪半径 |
yuki0701 | 0:3d9101dd0061 | 26 | double count_to_distance_mm_;//カウントを距離に変換する係数 |
yuki0701 | 0:3d9101dd0061 | 27 | double gear_rate_;//ギア比(エンコーダタイムプーリー/測定輪タイムプーリー) |
yuki0701 | 0:3d9101dd0061 | 28 | int is_diameter_set;//diameterがセットされていたら1 |
yuki0701 | 0:3d9101dd0061 | 29 | //z相用 |
yuki0701 | 0:3d9101dd0061 | 30 | bool first; |
yuki0701 | 0:3d9101dd0061 | 31 | int rev; |
yuki0701 | 0:3d9101dd0061 | 32 | double now_time,old_time; |
yuki0701 | 0:3d9101dd0061 | 33 | double RPM,RPM_old; |
yuki0701 | 0:3d9101dd0061 | 34 | int RPM_th; |
yuki0701 | 0:3d9101dd0061 | 35 | |
yuki0701 | 0:3d9101dd0061 | 36 | InterruptIn signalA_; |
yuki0701 | 0:3d9101dd0061 | 37 | InterruptIn signalB_; |
yuki0701 | 0:3d9101dd0061 | 38 | InterruptIn signalZ_; |
yuki0701 | 0:3d9101dd0061 | 39 | |
yuki0701 | 0:3d9101dd0061 | 40 | void upA(); |
yuki0701 | 0:3d9101dd0061 | 41 | void downA(); |
yuki0701 | 0:3d9101dd0061 | 42 | void upB(); |
yuki0701 | 0:3d9101dd0061 | 43 | void downB(); |
yuki0701 | 0:3d9101dd0061 | 44 | void upZ(); |
yuki0701 | 0:3d9101dd0061 | 45 | void downZ(); |
yuki0701 | 0:3d9101dd0061 | 46 | void setCountToDistance_mm();//countからdistanceに変換する係数を計算する。 |
yuki0701 | 0:3d9101dd0061 | 47 | public: |
yuki0701 | 0:3d9101dd0061 | 48 | double omega; //角速度 |
yuki0701 | 0:3d9101dd0061 | 49 | /** コンストラクタの定義 |
yuki0701 | 0:3d9101dd0061 | 50 | * |
yuki0701 | 0:3d9101dd0061 | 51 | * ***Z相の機能を追加したことで引数が増えました!!!!*** |
yuki0701 | 0:3d9101dd0061 | 52 | * |
yuki0701 | 0:3d9101dd0061 | 53 | * main関数の前に必ず一度宣言する |
yuki0701 | 0:3d9101dd0061 | 54 | * |
yuki0701 | 0:3d9101dd0061 | 55 | * 使うエンコーダの数だけ設定する必要がある |
yuki0701 | 0:3d9101dd0061 | 56 | * |
yuki0701 | 0:3d9101dd0061 | 57 | * @param signalA エンコーダA相のピン名 |
yuki0701 | 0:3d9101dd0061 | 58 | * @param signalB エンコーダB相のピン名 |
yuki0701 | 0:3d9101dd0061 | 59 | * @param signalZ エンコーダZ相のピン名 |
yuki0701 | 0:3d9101dd0061 | 60 | * @param s エンコーダの分解能(省略可) |
yuki0701 | 0:3d9101dd0061 | 61 | * @param t 角速度計算の間隔(省略可) |
yuki0701 | 0:3d9101dd0061 | 62 | */ |
yuki0701 | 0:3d9101dd0061 | 63 | /** @section CAUTION |
yuki0701 | 0:3d9101dd0061 | 64 | * 今まで以下のように定義していたものは |
yuki0701 | 0:3d9101dd0061 | 65 | * @code |
yuki0701 | 0:3d9101dd0061 | 66 | * #include "mbed.h" |
yuki0701 | 0:3d9101dd0061 | 67 | * #include "EC.h" |
yuki0701 | 0:3d9101dd0061 | 68 | * |
yuki0701 | 0:3d9101dd0061 | 69 | * Ec Ec1(PA_0,PA_1,1024,0.05); |
yuki0701 | 0:3d9101dd0061 | 70 | * @endcode |
yuki0701 | 0:3d9101dd0061 | 71 | * 次のようにZ相の引数の部分に、NCと入れれば今までの様に使える |
yuki0701 | 0:3d9101dd0061 | 72 | * @code |
yuki0701 | 0:3d9101dd0061 | 73 | * #include "mbed.h" |
yuki0701 | 0:3d9101dd0061 | 74 | * #include "EC.h" |
yuki0701 | 0:3d9101dd0061 | 75 | * |
yuki0701 | 0:3d9101dd0061 | 76 | * Ec Ec1(PA_0,PA_1,NC,1024,0.05); |
yuki0701 | 0:3d9101dd0061 | 77 | * @endcode |
yuki0701 | 0:3d9101dd0061 | 78 | */ |
yuki0701 | 0:3d9101dd0061 | 79 | Ec(PinName signalA,PinName signalB,PinName signalZ,int s,double t); |
yuki0701 | 0:3d9101dd0061 | 80 | |
yuki0701 | 0:3d9101dd0061 | 81 | ///countの値を返す関数(int型) |
yuki0701 | 0:3d9101dd0061 | 82 | int getCount(); |
yuki0701 | 0:3d9101dd0061 | 83 | |
yuki0701 | 0:3d9101dd0061 | 84 | /** omega(角速度 rad/s)の値を計算する関数 |
yuki0701 | 0:3d9101dd0061 | 85 | * @section CAUTION |
yuki0701 | 0:3d9101dd0061 | 86 | * CalOmega関数は、一定時間ごと(dtごと)に計算される必要があるので、main関数内でタイマー割り込みを設定する必要がある。 |
yuki0701 | 0:3d9101dd0061 | 87 | * @code |
yuki0701 | 0:3d9101dd0061 | 88 | * #include "mbed.h" |
yuki0701 | 0:3d9101dd0061 | 89 | * #include "EC.h" //ライブラリをインクルード |
yuki0701 | 0:3d9101dd0061 | 90 | * |
yuki0701 | 0:3d9101dd0061 | 91 | * Ec Ec1(PA_0,PA_1,NC,1024,0.05); //分解能1024、計算間隔0.05秒に設定、Z相は使わない |
yuki0701 | 0:3d9101dd0061 | 92 | * Ticker ticker; |
yuki0701 | 0:3d9101dd0061 | 93 | * DigitalIn button(USER_BUTTON); |
yuki0701 | 0:3d9101dd0061 | 94 | * Serial pc(USBTX,USBRX); |
yuki0701 | 0:3d9101dd0061 | 95 | * |
yuki0701 | 0:3d9101dd0061 | 96 | * void calOmega(){ |
yuki0701 | 0:3d9101dd0061 | 97 | * Ec1.CalOmega(); |
yuki0701 | 0:3d9101dd0061 | 98 | * } |
yuki0701 | 0:3d9101dd0061 | 99 | * |
yuki0701 | 0:3d9101dd0061 | 100 | * int main(){ |
yuki0701 | 0:3d9101dd0061 | 101 | * ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 |
yuki0701 | 0:3d9101dd0061 | 102 | * while(1){ |
yuki0701 | 0:3d9101dd0061 | 103 | * pc.printf(" count1=%d ",Ec1.getCount()); |
yuki0701 | 0:3d9101dd0061 | 104 | * pc.printf(" omega1=%f\r\n ",Ec1.getOmega()); |
yuki0701 | 0:3d9101dd0061 | 105 | * if(pc.readable()) { |
yuki0701 | 0:3d9101dd0061 | 106 | * char sel=pc.getc(); |
yuki0701 | 0:3d9101dd0061 | 107 | * if(sel=='r') Ec1.reset(); //rを押したらリセット |
yuki0701 | 0:3d9101dd0061 | 108 | * } |
yuki0701 | 0:3d9101dd0061 | 109 | * } |
yuki0701 | 0:3d9101dd0061 | 110 | * } |
yuki0701 | 0:3d9101dd0061 | 111 | * @endcode |
yuki0701 | 0:3d9101dd0061 | 112 | */ |
yuki0701 | 0:3d9101dd0061 | 113 | void CalOmega(); |
yuki0701 | 0:3d9101dd0061 | 114 | |
yuki0701 | 0:3d9101dd0061 | 115 | ///omega(角速度 rad/s)の値を返す関数(double型) |
yuki0701 | 0:3d9101dd0061 | 116 | double getOmega(); |
yuki0701 | 0:3d9101dd0061 | 117 | ///四倍精度のcountの値を返す関数(double型) |
yuki0701 | 0:3d9101dd0061 | 118 | double getPreCount(); |
yuki0701 | 0:3d9101dd0061 | 119 | ///エンコーダを初期状態に戻す関数 countやomegaの値を0にする |
yuki0701 | 0:3d9101dd0061 | 120 | virtual void reset(); |
yuki0701 | 0:3d9101dd0061 | 121 | ///角速度計算の間隔dtを決めることができる(デフォルトは0.05秒) |
yuki0701 | 0:3d9101dd0061 | 122 | void setTime(double t); |
yuki0701 | 0:3d9101dd0061 | 123 | ///(Z相を使用する場合)回転回数を返す関数(int型) |
yuki0701 | 0:3d9101dd0061 | 124 | int getRev(); |
yuki0701 | 0:3d9101dd0061 | 125 | |
yuki0701 | 0:3d9101dd0061 | 126 | ///(Z相を使用する場合)回転速度(rpm)の値を返す関数(double型) |
yuki0701 | 0:3d9101dd0061 | 127 | /** @section SAMPLE |
yuki0701 | 0:3d9101dd0061 | 128 | * z相を使う場合のプログラムの例 |
yuki0701 | 0:3d9101dd0061 | 129 | * @code |
yuki0701 | 0:3d9101dd0061 | 130 | * #include "mbed.h" |
yuki0701 | 0:3d9101dd0061 | 131 | * #include "EC.h" //ライブラリをインクルード |
yuki0701 | 0:3d9101dd0061 | 132 | * |
yuki0701 | 0:3d9101dd0061 | 133 | * Ec Ec1(NC,NC,PA_0,1024,0.05); //A相B相が不必要な場合も、このようにNCと入れればよい |
yuki0701 | 0:3d9101dd0061 | 134 | * DigitalIn button(USER_BUTTON); |
yuki0701 | 0:3d9101dd0061 | 135 | * Serial pc(USBTX,USBRX); |
yuki0701 | 0:3d9101dd0061 | 136 | * |
yuki0701 | 0:3d9101dd0061 | 137 | * int main(){ |
yuki0701 | 0:3d9101dd0061 | 138 | * while(1){ |
yuki0701 | 0:3d9101dd0061 | 139 | * pc.printf(" rev1=%d ",Ec1.getRev()); |
yuki0701 | 0:3d9101dd0061 | 140 | * pc.printf(" rpm1=%f\r\n ",Ec1.getRPM()); |
yuki0701 | 0:3d9101dd0061 | 141 | * if(!button) Ec1.reset(); //USERボタンを押したらリセット |
yuki0701 | 0:3d9101dd0061 | 142 | * } |
yuki0701 | 0:3d9101dd0061 | 143 | * } |
yuki0701 | 0:3d9101dd0061 | 144 | * @endcode |
yuki0701 | 0:3d9101dd0061 | 145 | */ |
yuki0701 | 0:3d9101dd0061 | 146 | double getRPM(); |
yuki0701 | 0:3d9101dd0061 | 147 | void changeRPM_th(int th); |
yuki0701 | 0:3d9101dd0061 | 148 | |
yuki0701 | 0:3d9101dd0061 | 149 | /** @caution 先にsetDiameter_mmをしないと正しい値とならない |
yuki0701 | 0:3d9101dd0061 | 150 | @return 現在距離. |
yuki0701 | 0:3d9101dd0061 | 151 | @code |
yuki0701 | 0:3d9101dd0061 | 152 | #include <mbed.h> |
yuki0701 | 0:3d9101dd0061 | 153 | #include "EC.h" |
yuki0701 | 0:3d9101dd0061 | 154 | Ec ec(PB_5, PB_4, NC, 500, 0.5); |
yuki0701 | 0:3d9101dd0061 | 155 | int main() |
yuki0701 | 0:3d9101dd0061 | 156 | { |
yuki0701 | 0:3d9101dd0061 | 157 | ec.setDiameter_mm(48); |
yuki0701 | 0:3d9101dd0061 | 158 | while(1) { |
yuki0701 | 0:3d9101dd0061 | 159 | printf("%f\r\n", ec.getDistance_mm()); |
yuki0701 | 0:3d9101dd0061 | 160 | wait(0.1); |
yuki0701 | 0:3d9101dd0061 | 161 | } |
yuki0701 | 0:3d9101dd0061 | 162 | } |
yuki0701 | 0:3d9101dd0061 | 163 | @endcode |
yuki0701 | 0:3d9101dd0061 | 164 | */ |
yuki0701 | 0:3d9101dd0061 | 165 | double getDistance_mm(); |
yuki0701 | 0:3d9101dd0061 | 166 | |
yuki0701 | 0:3d9101dd0061 | 167 | /** @param diameter_mm 測定輪半径 |
yuki0701 | 0:3d9101dd0061 | 168 | */ |
yuki0701 | 0:3d9101dd0061 | 169 | void setDiameter_mm(double diameter_mm); |
yuki0701 | 0:3d9101dd0061 | 170 | /** @param gear比(エンコーダタイムプーリー/測定輪タイムプーリー) |
yuki0701 | 0:3d9101dd0061 | 171 | */ |
yuki0701 | 0:3d9101dd0061 | 172 | void setGearRate(double gear_rate); |
yuki0701 | 0:3d9101dd0061 | 173 | static double deftime;//角速度計算の間隔 |
yuki0701 | 0:3d9101dd0061 | 174 | static int defsolution;//エンコーダの分解能のデフォルト値 |
yuki0701 | 0:3d9101dd0061 | 175 | Timer timer; |
yuki0701 | 0:3d9101dd0061 | 176 | }; |
yuki0701 | 0:3d9101dd0061 | 177 | |
yuki0701 | 0:3d9101dd0061 | 178 | |
yuki0701 | 0:3d9101dd0061 | 179 | #endif |