春ロボ ロケット団
test
EC.cpp@0:eb20667a5a27, 2019-07-04 (annotated)
- Committer:
- MazeTaka
- Date:
- Thu Jul 04 09:25:36 2019 +0000
- Revision:
- 0:eb20667a5a27
- Child:
- 1:b7e957c01761
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| MazeTaka | 0:eb20667a5a27 | 1 | #include "mbed.h" |
| MazeTaka | 0:eb20667a5a27 | 2 | #include "EC.h" |
| MazeTaka | 0:eb20667a5a27 | 3 | |
| MazeTaka | 0:eb20667a5a27 | 4 | |
| MazeTaka | 0:eb20667a5a27 | 5 | Ec::Ec(int res,int multi): |
| MazeTaka | 0:eb20667a5a27 | 6 | count_(0),pre_count_(0),resolution_(res),multiplication_(multi) |
| MazeTaka | 0:eb20667a5a27 | 7 | { |
| MazeTaka | 0:eb20667a5a27 | 8 | timer_.start(); |
| MazeTaka | 0:eb20667a5a27 | 9 | } |
| MazeTaka | 0:eb20667a5a27 | 10 | |
| MazeTaka | 0:eb20667a5a27 | 11 | int Ec::getCount() |
| MazeTaka | 0:eb20667a5a27 | 12 | { |
| MazeTaka | 0:eb20667a5a27 | 13 | return count_; |
| MazeTaka | 0:eb20667a5a27 | 14 | } |
| MazeTaka | 0:eb20667a5a27 | 15 | |
| MazeTaka | 0:eb20667a5a27 | 16 | void Ec::calOmega() |
| MazeTaka | 0:eb20667a5a27 | 17 | { |
| MazeTaka | 0:eb20667a5a27 | 18 | double t=timer_.read(); |
| MazeTaka | 0:eb20667a5a27 | 19 | omega_=(count_-pre_count_)*2.0f*M_PI/(multiplication_*resolution_*(t-ptw_)); |
| MazeTaka | 0:eb20667a5a27 | 20 | pre_count_=count_; |
| MazeTaka | 0:eb20667a5a27 | 21 | ptw_=t; |
| MazeTaka | 0:eb20667a5a27 | 22 | } |
| MazeTaka | 0:eb20667a5a27 | 23 | |
| MazeTaka | 0:eb20667a5a27 | 24 | double Ec::getOmega() |
| MazeTaka | 0:eb20667a5a27 | 25 | { |
| MazeTaka | 0:eb20667a5a27 | 26 | return omega_; |
| MazeTaka | 0:eb20667a5a27 | 27 | } |
| MazeTaka | 0:eb20667a5a27 | 28 | void Ec::setResolution(int res){ |
| MazeTaka | 0:eb20667a5a27 | 29 | resolution_=res; |
| MazeTaka | 0:eb20667a5a27 | 30 | } |
| MazeTaka | 0:eb20667a5a27 | 31 | |
| MazeTaka | 0:eb20667a5a27 | 32 | /*reset関数の定義*/ |
| MazeTaka | 0:eb20667a5a27 | 33 | /*エンコーダを初期状態に戻すことができる*/ |
| MazeTaka | 0:eb20667a5a27 | 34 | void Ec::reset() |
| MazeTaka | 0:eb20667a5a27 | 35 | { |
| MazeTaka | 0:eb20667a5a27 | 36 | count_=0; |
| MazeTaka | 0:eb20667a5a27 | 37 | pre_count_=0,omega_=0; |
| MazeTaka | 0:eb20667a5a27 | 38 | ptw_=0; |
| MazeTaka | 0:eb20667a5a27 | 39 | } |
| MazeTaka | 0:eb20667a5a27 | 40 | |
| MazeTaka | 0:eb20667a5a27 | 41 | |
| MazeTaka | 0:eb20667a5a27 | 42 | ////////////////////////////////////////////////////1逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 43 | Ec1multi::Ec1multi(PinName signalA,PinName signalB,int res) : Ec(res,1),signalA_(signalA),signalB_(signalB) |
| MazeTaka | 0:eb20667a5a27 | 44 | { |
| MazeTaka | 0:eb20667a5a27 | 45 | signalA_.rise(callback(this,&Ec1multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 46 | } |
| MazeTaka | 0:eb20667a5a27 | 47 | |
| MazeTaka | 0:eb20667a5a27 | 48 | //ピン変化割り込み関数の定義 |
| MazeTaka | 0:eb20667a5a27 | 49 | void Ec1multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 50 | { |
| MazeTaka | 0:eb20667a5a27 | 51 | if(signalB_.read())count_++; |
| MazeTaka | 0:eb20667a5a27 | 52 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 53 | } |
| MazeTaka | 0:eb20667a5a27 | 54 | ////////////////////////////////////////////////////2逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 55 | Ec2multi::Ec2multi(PinName signalA,PinName signalB,int res) : Ec(res,2),signalA_(signalA),signalB_(signalB) |
| MazeTaka | 0:eb20667a5a27 | 56 | { |
| MazeTaka | 0:eb20667a5a27 | 57 | signalA_.rise(callback(this,&Ec2multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 58 | signalA_.fall(callback(this,&Ec2multi::downA)); |
| MazeTaka | 0:eb20667a5a27 | 59 | printf("Initialize OK!!\r\n"); |
| MazeTaka | 0:eb20667a5a27 | 60 | } |
| MazeTaka | 0:eb20667a5a27 | 61 | |
| MazeTaka | 0:eb20667a5a27 | 62 | //ピン変化割り込み関数の定義 |
| MazeTaka | 0:eb20667a5a27 | 63 | void Ec2multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 64 | { |
| MazeTaka | 0:eb20667a5a27 | 65 | if(signalB_.read())count_++; |
| MazeTaka | 0:eb20667a5a27 | 66 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 67 | } |
| MazeTaka | 0:eb20667a5a27 | 68 | void Ec2multi::downA() |
| MazeTaka | 0:eb20667a5a27 | 69 | { |
| MazeTaka | 0:eb20667a5a27 | 70 | if(signalB_.read())count_--; |
| MazeTaka | 0:eb20667a5a27 | 71 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 72 | } |
| MazeTaka | 0:eb20667a5a27 | 73 | |
| MazeTaka | 0:eb20667a5a27 | 74 | ////////////////////////////////////////////////////4逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 75 | Ec4multi::Ec4multi(PinName signalA,PinName signalB,int res) : Ec(res,4),signalA_(signalA),signalB_(signalB),pa_(0),pb_(0) |
| MazeTaka | 0:eb20667a5a27 | 76 | { |
| MazeTaka | 0:eb20667a5a27 | 77 | signalA_.rise(callback(this,&Ec4multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 78 | signalA_.fall(callback(this,&Ec4multi::downA)); |
| MazeTaka | 0:eb20667a5a27 | 79 | signalB_.rise(callback(this,&Ec4multi::upB)); |
| MazeTaka | 0:eb20667a5a27 | 80 | signalB_.fall(callback(this,&Ec4multi::downB)); |
| MazeTaka | 0:eb20667a5a27 | 81 | } |
| MazeTaka | 0:eb20667a5a27 | 82 | void Ec4multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 83 | { |
| MazeTaka | 0:eb20667a5a27 | 84 | pa_=1; |
| MazeTaka | 0:eb20667a5a27 | 85 | if(pb_==1)count_++; |
| MazeTaka | 0:eb20667a5a27 | 86 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 87 | } |
| MazeTaka | 0:eb20667a5a27 | 88 | void Ec4multi::downA() |
| MazeTaka | 0:eb20667a5a27 | 89 | { |
| MazeTaka | 0:eb20667a5a27 | 90 | pa_=0; |
| MazeTaka | 0:eb20667a5a27 | 91 | if(pb_==1)count_--; |
| MazeTaka | 0:eb20667a5a27 | 92 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 93 | } |
| MazeTaka | 0:eb20667a5a27 | 94 | void Ec4multi::upB() |
| MazeTaka | 0:eb20667a5a27 | 95 | { |
| MazeTaka | 0:eb20667a5a27 | 96 | pb_=1; |
| MazeTaka | 0:eb20667a5a27 | 97 | if(pa_==1)count_--; |
| MazeTaka | 0:eb20667a5a27 | 98 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 99 | } |
| MazeTaka | 0:eb20667a5a27 | 100 | void Ec4multi::downB() |
| MazeTaka | 0:eb20667a5a27 | 101 | { |
| MazeTaka | 0:eb20667a5a27 | 102 | pb_=0; |
| MazeTaka | 0:eb20667a5a27 | 103 | if(pa_==1)count_++; |
| MazeTaka | 0:eb20667a5a27 | 104 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 105 | } |