春ロボ1班(元F3RC4班+) / PathFollowing1-ver2

春ロボ2019 経路追従プログラム（ver.2）

Dependencies:   CruizCore_R1370P

Fork of PathFollowing1 by 春ロボ1班(元F3RC4班+)

EC.h@1:9858517eef98, 2018-10-31 (annotated)

Committer:
yuki0701
Date:
Wed Oct 31 10:13:49 2018 +0000
Revision:
1:9858517eef98
Parent:
0:3d9101dd0061 
a;

Who changed what in which revision?

UserRevisionLine numberNew 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