ロボステ6期
ai
Dependents: ball_snatch NHK2020-arm-sub NHK2020-arm-sub NHK2020-arm-sub2 ... more
EC.cpp@1:b7e957c01761, 2019-07-04 (annotated)
- Committer:
- MazeTaka
- Date:
- Thu Jul 04 11:40:32 2019 +0000
- Revision:
- 1:b7e957c01761
- Parent:
- 0:eb20667a5a27
- Child:
- 8:17aff8f21d19
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 | 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 | printf("Initialize OK!!\r\n"); |
| MazeTaka | 0:eb20667a5a27 | 63 | } |
| MazeTaka | 0:eb20667a5a27 | 64 | |
| MazeTaka | 0:eb20667a5a27 | 65 | //ピン変化割り込み関数の定義 |
| MazeTaka | 0:eb20667a5a27 | 66 | void Ec2multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 67 | { |
| MazeTaka | 0:eb20667a5a27 | 68 | if(signalB_.read())count_++; |
| MazeTaka | 0:eb20667a5a27 | 69 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 70 | } |
| MazeTaka | 0:eb20667a5a27 | 71 | void Ec2multi::downA() |
| MazeTaka | 0:eb20667a5a27 | 72 | { |
| MazeTaka | 0:eb20667a5a27 | 73 | if(signalB_.read())count_--; |
| MazeTaka | 0:eb20667a5a27 | 74 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 75 | } |
| MazeTaka | 0:eb20667a5a27 | 76 | |
| MazeTaka | 0:eb20667a5a27 | 77 | ////////////////////////////////////////////////////4逓倍////////////////////////////////////////////////////////////////// |
| MazeTaka | 0:eb20667a5a27 | 78 | Ec4multi::Ec4multi(PinName signalA,PinName signalB,int res) : Ec(res,4),signalA_(signalA),signalB_(signalB),pa_(0),pb_(0) |
| MazeTaka | 0:eb20667a5a27 | 79 | { |
| MazeTaka | 0:eb20667a5a27 | 80 | signalA_.rise(callback(this,&Ec4multi::upA)); |
| MazeTaka | 0:eb20667a5a27 | 81 | signalA_.fall(callback(this,&Ec4multi::downA)); |
| MazeTaka | 0:eb20667a5a27 | 82 | signalB_.rise(callback(this,&Ec4multi::upB)); |
| MazeTaka | 0:eb20667a5a27 | 83 | signalB_.fall(callback(this,&Ec4multi::downB)); |
| MazeTaka | 0:eb20667a5a27 | 84 | } |
| MazeTaka | 0:eb20667a5a27 | 85 | void Ec4multi::upA() |
| MazeTaka | 0:eb20667a5a27 | 86 | { |
| MazeTaka | 0:eb20667a5a27 | 87 | pa_=1; |
| MazeTaka | 0:eb20667a5a27 | 88 | if(pb_==1)count_++; |
| MazeTaka | 0:eb20667a5a27 | 89 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 90 | } |
| MazeTaka | 0:eb20667a5a27 | 91 | void Ec4multi::downA() |
| MazeTaka | 0:eb20667a5a27 | 92 | { |
| MazeTaka | 0:eb20667a5a27 | 93 | pa_=0; |
| MazeTaka | 0:eb20667a5a27 | 94 | if(pb_==1)count_--; |
| MazeTaka | 0:eb20667a5a27 | 95 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 96 | } |
| MazeTaka | 0:eb20667a5a27 | 97 | void Ec4multi::upB() |
| MazeTaka | 0:eb20667a5a27 | 98 | { |
| MazeTaka | 0:eb20667a5a27 | 99 | pb_=1; |
| MazeTaka | 0:eb20667a5a27 | 100 | if(pa_==1)count_--; |
| MazeTaka | 0:eb20667a5a27 | 101 | else count_++; |
| MazeTaka | 0:eb20667a5a27 | 102 | } |
| MazeTaka | 0:eb20667a5a27 | 103 | void Ec4multi::downB() |
| MazeTaka | 0:eb20667a5a27 | 104 | { |
| MazeTaka | 0:eb20667a5a27 | 105 | pb_=0; |
| MazeTaka | 0:eb20667a5a27 | 106 | if(pa_==1)count_++; |
| MazeTaka | 0:eb20667a5a27 | 107 | else count_--; |
| MazeTaka | 0:eb20667a5a27 | 108 | } |