ロボステ6期
ai
Dependents: ball_snatch NHK2020-arm-sub NHK2020-arm-sub NHK2020-arm-sub2 ... more
EC.cpp@8:17aff8f21d19, 2019-07-24 (annotated)
- Committer:
- MazeTaka
- Date:
- Wed Jul 24 17:10:43 2019 +0000
- Revision:
- 8:17aff8f21d19
- Parent:
- 1:b7e957c01761
- Child:
- 10:ad0f13db7b92
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 | 8:17aff8f21d19 | 11 | int Ec::getCount()const |
| 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 | 8:17aff8f21d19 | 24 | double Ec::getOmega()const |
| 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 | 1:b7e957c01761 | 39 | timer_.stop(); |
| MazeTaka | 1:b7e957c01761 | 40 | timer_.reset(); |
| MazeTaka | 1:b7e957c01761 | 41 | timer_.start(); |
| MazeTaka | 0:eb20667a5a27 | 42 | } |
| MazeTaka | 0:eb20667a5a27 | 43 | |
| MazeTaka | 0:eb20667a5a27 | 44 | |
| MazeTaka | 0:eb20667a5a27 | 45 | ////////////////////////////////////////////////////1逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 46 | Ec1multi::Ec1multi(PinName signalA,PinName signalB,int res) : Ec(res,1),signalA_(signalA),signalB_(signalB) |
| MazeTaka | 0:eb20667a5a27 | 47 | { |
| MazeTaka | 0:eb20667a5a27 | 48 | signalA_.rise(callback(this,&Ec1multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 49 | } |
| MazeTaka | 0:eb20667a5a27 | 50 | |
| MazeTaka | 0:eb20667a5a27 | 51 | //ピン変化割り込み関数の定義 |
| MazeTaka | 0:eb20667a5a27 | 52 | void Ec1multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 53 | { |
| MazeTaka | 0:eb20667a5a27 | 54 | if(signalB_.read())count_++; |
| MazeTaka | 0:eb20667a5a27 | 55 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 56 | } |
| MazeTaka | 0:eb20667a5a27 | 57 | ////////////////////////////////////////////////////2逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 58 | Ec2multi::Ec2multi(PinName signalA,PinName signalB,int res) : Ec(res,2),signalA_(signalA),signalB_(signalB) |
| MazeTaka | 0:eb20667a5a27 | 59 | { |
| MazeTaka | 0:eb20667a5a27 | 60 | signalA_.rise(callback(this,&Ec2multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 61 | signalA_.fall(callback(this,&Ec2multi::downA)); |
| MazeTaka | 0:eb20667a5a27 | 62 | } |
| MazeTaka | 0:eb20667a5a27 | 63 | |
| MazeTaka | 0:eb20667a5a27 | 64 | //ピン変化割り込み関数の定義 |
| MazeTaka | 0:eb20667a5a27 | 65 | void Ec2multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 66 | { |
| MazeTaka | 0:eb20667a5a27 | 67 | if(signalB_.read())count_++; |
| MazeTaka | 0:eb20667a5a27 | 68 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 69 | } |
| MazeTaka | 0:eb20667a5a27 | 70 | void Ec2multi::downA() |
| MazeTaka | 0:eb20667a5a27 | 71 | { |
| MazeTaka | 0:eb20667a5a27 | 72 | if(signalB_.read())count_--; |
| MazeTaka | 0:eb20667a5a27 | 73 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 74 | } |
| MazeTaka | 0:eb20667a5a27 | 75 | |
| MazeTaka | 0:eb20667a5a27 | 76 | ////////////////////////////////////////////////////4逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 77 | Ec4multi::Ec4multi(PinName signalA,PinName signalB,int res) : Ec(res,4),signalA_(signalA),signalB_(signalB),pa_(0),pb_(0) |
| MazeTaka | 0:eb20667a5a27 | 78 | { |
| MazeTaka | 0:eb20667a5a27 | 79 | signalA_.rise(callback(this,&Ec4multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 80 | signalA_.fall(callback(this,&Ec4multi::downA)); |
| MazeTaka | 0:eb20667a5a27 | 81 | signalB_.rise(callback(this,&Ec4multi::upB)); |
| MazeTaka | 0:eb20667a5a27 | 82 | signalB_.fall(callback(this,&Ec4multi::downB)); |
| MazeTaka | 0:eb20667a5a27 | 83 | } |
| MazeTaka | 0:eb20667a5a27 | 84 | void Ec4multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 85 | { |
| MazeTaka | 0:eb20667a5a27 | 86 | pa_=1; |
| MazeTaka | 0:eb20667a5a27 | 87 | if(pb_==1)count_++; |
| MazeTaka | 0:eb20667a5a27 | 88 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 89 | } |
| MazeTaka | 0:eb20667a5a27 | 90 | void Ec4multi::downA() |
| MazeTaka | 0:eb20667a5a27 | 91 | { |
| MazeTaka | 0:eb20667a5a27 | 92 | pa_=0; |
| MazeTaka | 0:eb20667a5a27 | 93 | if(pb_==1)count_--; |
| MazeTaka | 0:eb20667a5a27 | 94 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 95 | } |
| MazeTaka | 0:eb20667a5a27 | 96 | void Ec4multi::upB() |
| MazeTaka | 0:eb20667a5a27 | 97 | { |
| MazeTaka | 0:eb20667a5a27 | 98 | pb_=1; |
| MazeTaka | 0:eb20667a5a27 | 99 | if(pa_==1)count_--; |
| MazeTaka | 0:eb20667a5a27 | 100 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 101 | } |
| MazeTaka | 0:eb20667a5a27 | 102 | void Ec4multi::downB() |
| MazeTaka | 0:eb20667a5a27 | 103 | { |
| MazeTaka | 0:eb20667a5a27 | 104 | pb_=0; |
| MazeTaka | 0:eb20667a5a27 | 105 | if(pa_==1)count_++; |
| MazeTaka | 0:eb20667a5a27 | 106 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 107 | } |