![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
明石高専ロボ研 mbedライブラリ
Dependencies: mbed
rotary_inc.cpp@0:ca84ed7518f5, 2020-01-02 (annotated)
- Committer:
- TanakaRobo
- Date:
- Thu Jan 02 09:30:06 2020 +0000
- Revision:
- 0:ca84ed7518f5
- Child:
- 3:28c77df7c0b6
buffered serial before test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TanakaRobo | 0:ca84ed7518f5 | 1 | #include "rotary_inc.hpp" |
TanakaRobo | 0:ca84ed7518f5 | 2 | |
TanakaRobo | 0:ca84ed7518f5 | 3 | RotaryInc::RotaryInc(PinName pinA, PinName pinB,int mode):mode_(mode){ |
TanakaRobo | 0:ca84ed7518f5 | 4 | measur_ = false; |
TanakaRobo | 0:ca84ed7518f5 | 5 | init(pinA,pinB); |
TanakaRobo | 0:ca84ed7518f5 | 6 | } |
TanakaRobo | 0:ca84ed7518f5 | 7 | |
TanakaRobo | 0:ca84ed7518f5 | 8 | RotaryInc::RotaryInc(PinName pinA,PinName pinB,double circumference,int resolution,int mode) |
TanakaRobo | 0:ca84ed7518f5 | 9 | :mode_(mode),resolution_(resolution),circumference_(circumference){ |
TanakaRobo | 0:ca84ed7518f5 | 10 | measur_ = true; |
TanakaRobo | 0:ca84ed7518f5 | 11 | init(pinA,pinB); |
TanakaRobo | 0:ca84ed7518f5 | 12 | } |
TanakaRobo | 0:ca84ed7518f5 | 13 | |
TanakaRobo | 0:ca84ed7518f5 | 14 | void RotaryInc::init(PinName pinA,PinName pinB){ |
TanakaRobo | 0:ca84ed7518f5 | 15 | reset(); |
TanakaRobo | 0:ca84ed7518f5 | 16 | A_ = new InterruptIn(pinA,PullUp); |
TanakaRobo | 0:ca84ed7518f5 | 17 | B_ = new InterruptIn(pinB,PullUp); |
TanakaRobo | 0:ca84ed7518f5 | 18 | A_->rise(callback(this,&RotaryInc::riseA)); |
TanakaRobo | 0:ca84ed7518f5 | 19 | |
TanakaRobo | 0:ca84ed7518f5 | 20 | if(mode_ == 2){ |
TanakaRobo | 0:ca84ed7518f5 | 21 | A_->fall(callback(this,&RotaryInc::fallA)); |
TanakaRobo | 0:ca84ed7518f5 | 22 | }else if(mode_ == 4){ |
TanakaRobo | 0:ca84ed7518f5 | 23 | A_->fall(callback(this,&RotaryInc::fallA)); |
TanakaRobo | 0:ca84ed7518f5 | 24 | B_->rise(callback(this,&RotaryInc::riseB)); |
TanakaRobo | 0:ca84ed7518f5 | 25 | B_->fall(callback(this,&RotaryInc::fallB)); |
TanakaRobo | 0:ca84ed7518f5 | 26 | }else{ |
TanakaRobo | 0:ca84ed7518f5 | 27 | mode_ = 1; |
TanakaRobo | 0:ca84ed7518f5 | 28 | } |
TanakaRobo | 0:ca84ed7518f5 | 29 | } |
TanakaRobo | 0:ca84ed7518f5 | 30 | |
TanakaRobo | 0:ca84ed7518f5 | 31 | void RotaryInc::zero(){ |
TanakaRobo | 0:ca84ed7518f5 | 32 | timer_.stop(); |
TanakaRobo | 0:ca84ed7518f5 | 33 | timer_.reset(); |
TanakaRobo | 0:ca84ed7518f5 | 34 | start_frag_ = false; |
TanakaRobo | 0:ca84ed7518f5 | 35 | flag_ = false; |
TanakaRobo | 0:ca84ed7518f5 | 36 | last_[0] = pulse_; |
TanakaRobo | 0:ca84ed7518f5 | 37 | speed_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 38 | count_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 39 | sum_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 40 | now_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 41 | } |
TanakaRobo | 0:ca84ed7518f5 | 42 | |
TanakaRobo | 0:ca84ed7518f5 | 43 | void RotaryInc::calcu(){ |
TanakaRobo | 0:ca84ed7518f5 | 44 | if(!start_frag_){ |
TanakaRobo | 0:ca84ed7518f5 | 45 | timer_.start(); |
TanakaRobo | 0:ca84ed7518f5 | 46 | start_frag_ = true; |
TanakaRobo | 0:ca84ed7518f5 | 47 | last_[0] = pulse_; |
TanakaRobo | 0:ca84ed7518f5 | 48 | pre_t_[0] = 0; |
TanakaRobo | 0:ca84ed7518f5 | 49 | count_ = 1; |
TanakaRobo | 0:ca84ed7518f5 | 50 | }else if(flag_){ |
TanakaRobo | 0:ca84ed7518f5 | 51 | now_ = timer_.read(); |
TanakaRobo | 0:ca84ed7518f5 | 52 | timer_.reset(); |
TanakaRobo | 0:ca84ed7518f5 | 53 | sum_ -= pre_t_[count_]; |
TanakaRobo | 0:ca84ed7518f5 | 54 | pre_t_[count_] = now_; |
TanakaRobo | 0:ca84ed7518f5 | 55 | sum_ += now_; |
TanakaRobo | 0:ca84ed7518f5 | 56 | speed_ = (double)(pulse_ - last_[count_]) / sum_; |
TanakaRobo | 0:ca84ed7518f5 | 57 | last_[count_] = pulse_; |
TanakaRobo | 0:ca84ed7518f5 | 58 | if(count_ < 19){ |
TanakaRobo | 0:ca84ed7518f5 | 59 | count_++; |
TanakaRobo | 0:ca84ed7518f5 | 60 | }else{ |
TanakaRobo | 0:ca84ed7518f5 | 61 | count_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 62 | } |
TanakaRobo | 0:ca84ed7518f5 | 63 | }else{ |
TanakaRobo | 0:ca84ed7518f5 | 64 | now_ = timer_.read(); |
TanakaRobo | 0:ca84ed7518f5 | 65 | timer_.reset(); |
TanakaRobo | 0:ca84ed7518f5 | 66 | pre_t_[count_] = now_; |
TanakaRobo | 0:ca84ed7518f5 | 67 | sum_ += now_; |
TanakaRobo | 0:ca84ed7518f5 | 68 | speed_ = (double)(pulse_ - last_[0]) / sum_; |
TanakaRobo | 0:ca84ed7518f5 | 69 | last_[count_] = pulse_; |
TanakaRobo | 0:ca84ed7518f5 | 70 | count_++; |
TanakaRobo | 0:ca84ed7518f5 | 71 | if(count_ > 19){ |
TanakaRobo | 0:ca84ed7518f5 | 72 | count_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 73 | flag_ = true; |
TanakaRobo | 0:ca84ed7518f5 | 74 | } |
TanakaRobo | 0:ca84ed7518f5 | 75 | } |
TanakaRobo | 0:ca84ed7518f5 | 76 | } |
TanakaRobo | 0:ca84ed7518f5 | 77 | |
TanakaRobo | 0:ca84ed7518f5 | 78 | void RotaryInc::riseA(){ |
TanakaRobo | 0:ca84ed7518f5 | 79 | B_->read() ? pulse_-- : pulse_++; |
TanakaRobo | 0:ca84ed7518f5 | 80 | if(measur_){ |
TanakaRobo | 0:ca84ed7518f5 | 81 | calcu(); |
TanakaRobo | 0:ca84ed7518f5 | 82 | } |
TanakaRobo | 0:ca84ed7518f5 | 83 | } |
TanakaRobo | 0:ca84ed7518f5 | 84 | |
TanakaRobo | 0:ca84ed7518f5 | 85 | void RotaryInc::fallA(){ |
TanakaRobo | 0:ca84ed7518f5 | 86 | B_->read() ? pulse_++ : pulse_--; |
TanakaRobo | 0:ca84ed7518f5 | 87 | if(measur_){ |
TanakaRobo | 0:ca84ed7518f5 | 88 | calcu(); |
TanakaRobo | 0:ca84ed7518f5 | 89 | } |
TanakaRobo | 0:ca84ed7518f5 | 90 | } |
TanakaRobo | 0:ca84ed7518f5 | 91 | |
TanakaRobo | 0:ca84ed7518f5 | 92 | void RotaryInc::riseB(){ |
TanakaRobo | 0:ca84ed7518f5 | 93 | A_->read() ? pulse_++ : pulse_--; |
TanakaRobo | 0:ca84ed7518f5 | 94 | if(measur_){ |
TanakaRobo | 0:ca84ed7518f5 | 95 | calcu(); |
TanakaRobo | 0:ca84ed7518f5 | 96 | } |
TanakaRobo | 0:ca84ed7518f5 | 97 | } |
TanakaRobo | 0:ca84ed7518f5 | 98 | |
TanakaRobo | 0:ca84ed7518f5 | 99 | void RotaryInc::fallB(){ |
TanakaRobo | 0:ca84ed7518f5 | 100 | A_->read() ? pulse_-- : pulse_++; |
TanakaRobo | 0:ca84ed7518f5 | 101 | if(measur_){ |
TanakaRobo | 0:ca84ed7518f5 | 102 | calcu(); |
TanakaRobo | 0:ca84ed7518f5 | 103 | } |
TanakaRobo | 0:ca84ed7518f5 | 104 | } |
TanakaRobo | 0:ca84ed7518f5 | 105 | |
TanakaRobo | 0:ca84ed7518f5 | 106 | long long RotaryInc::get(){ |
TanakaRobo | 0:ca84ed7518f5 | 107 | return pulse_; |
TanakaRobo | 0:ca84ed7518f5 | 108 | } |
TanakaRobo | 0:ca84ed7518f5 | 109 | |
TanakaRobo | 0:ca84ed7518f5 | 110 | double RotaryInc::getSpeed(){ |
TanakaRobo | 0:ca84ed7518f5 | 111 | if(!measur_){ |
TanakaRobo | 0:ca84ed7518f5 | 112 | return 0; |
TanakaRobo | 0:ca84ed7518f5 | 113 | } |
TanakaRobo | 0:ca84ed7518f5 | 114 | if(timer_.read_ms() > 150){ |
TanakaRobo | 0:ca84ed7518f5 | 115 | zero(); |
TanakaRobo | 0:ca84ed7518f5 | 116 | } |
TanakaRobo | 0:ca84ed7518f5 | 117 | return speed_ / resolution_ / mode_ * circumference_; |
TanakaRobo | 0:ca84ed7518f5 | 118 | } |
TanakaRobo | 0:ca84ed7518f5 | 119 | |
TanakaRobo | 0:ca84ed7518f5 | 120 | int RotaryInc::diff(){ |
TanakaRobo | 0:ca84ed7518f5 | 121 | int diff = pulse_ - prev_; |
TanakaRobo | 0:ca84ed7518f5 | 122 | prev_ = pulse_; |
TanakaRobo | 0:ca84ed7518f5 | 123 | return diff; |
TanakaRobo | 0:ca84ed7518f5 | 124 | } |
TanakaRobo | 0:ca84ed7518f5 | 125 | |
TanakaRobo | 0:ca84ed7518f5 | 126 | void RotaryInc::reset(){ |
TanakaRobo | 0:ca84ed7518f5 | 127 | pulse_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 128 | prev_ = 0; |
TanakaRobo | 0:ca84ed7518f5 | 129 | if(measur_){ |
TanakaRobo | 0:ca84ed7518f5 | 130 | zero(); |
TanakaRobo | 0:ca84ed7518f5 | 131 | } |
TanakaRobo | 0:ca84ed7518f5 | 132 | } |
TanakaRobo | 0:ca84ed7518f5 | 133 | |
TanakaRobo | 0:ca84ed7518f5 | 134 | RotaryInc::~RotaryInc(){ |
TanakaRobo | 0:ca84ed7518f5 | 135 | A_->disable_irq(); |
TanakaRobo | 0:ca84ed7518f5 | 136 | B_->disable_irq(); |
TanakaRobo | 0:ca84ed7518f5 | 137 | delete A_; |
TanakaRobo | 0:ca84ed7518f5 | 138 | delete B_; |
TanakaRobo | 0:ca84ed7518f5 | 139 | } |