![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
my new gear...
sensor/rotary_inc_sp.cpp@22:394337a4205a, 22 months ago (annotated)
- Committer:
- yootee
- Date:
- Sat Oct 15 07:52:25 2022 +0000
- Revision:
- 22:394337a4205a
- Parent:
- 2:e7b09385d197
upgrade
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yootee | 0:1456b6f84c75 | 1 | #include "rotary_inc_sp.hpp" |
yootee | 0:1456b6f84c75 | 2 | |
yootee | 0:1456b6f84c75 | 3 | RotaryInc::RotaryInc(PinName pin_a, PinName pin_b,int mode):mode_(mode){ |
yootee | 0:1456b6f84c75 | 4 | measur_ = false; |
yootee | 0:1456b6f84c75 | 5 | init(pin_a,pin_b); |
yootee | 0:1456b6f84c75 | 6 | } |
yootee | 0:1456b6f84c75 | 7 | |
yootee | 0:1456b6f84c75 | 8 | RotaryInc::RotaryInc(PinName pin_a,PinName pin_b,double circumference,int resolution,int mode) |
yootee | 0:1456b6f84c75 | 9 | :mode_(mode),resolution_(resolution),circumference_(circumference){ |
yootee | 0:1456b6f84c75 | 10 | measur_ = true; |
yootee | 0:1456b6f84c75 | 11 | init(pin_a,pin_b); |
yootee | 0:1456b6f84c75 | 12 | } |
yootee | 0:1456b6f84c75 | 13 | |
yootee | 22:394337a4205a | 14 | RotaryInc::RotaryInc(Port port,double circumference,int resolution,int mode) |
yootee | 22:394337a4205a | 15 | :mode_(mode),resolution_(resolution),circumference_(circumference){ |
yootee | 22:394337a4205a | 16 | measur_ = true; |
yootee | 22:394337a4205a | 17 | init(port.pin[0],port.pin[1]); |
yootee | 22:394337a4205a | 18 | } |
yootee | 22:394337a4205a | 19 | |
yootee | 0:1456b6f84c75 | 20 | RotaryInc::RotaryInc(){ |
yootee | 0:1456b6f84c75 | 21 | measur_ = true; |
yootee | 0:1456b6f84c75 | 22 | }; |
yootee | 0:1456b6f84c75 | 23 | |
yootee | 0:1456b6f84c75 | 24 | void RotaryInc::init(PinName pinA,PinName pinB){ |
yootee | 0:1456b6f84c75 | 25 | reset(); |
yootee | 0:1456b6f84c75 | 26 | pin_a_ = new InterruptIn(pinA,PullUp); |
yootee | 0:1456b6f84c75 | 27 | pin_b_ = new InterruptIn(pinB,PullUp); |
yootee | 0:1456b6f84c75 | 28 | pin_a_->rise(callback(this,&RotaryInc::riseA)); |
yootee | 0:1456b6f84c75 | 29 | |
yootee | 0:1456b6f84c75 | 30 | if(mode_ == 2){ |
yootee | 0:1456b6f84c75 | 31 | pin_b_->rise(callback(this,&RotaryInc::riseB)); |
yootee | 0:1456b6f84c75 | 32 | }else if(mode_ == 4){ |
yootee | 0:1456b6f84c75 | 33 | pin_a_->fall(callback(this,&RotaryInc::fallA)); |
yootee | 0:1456b6f84c75 | 34 | pin_b_->rise(callback(this,&RotaryInc::riseB)); |
yootee | 0:1456b6f84c75 | 35 | pin_b_->fall(callback(this,&RotaryInc::fallB)); |
yootee | 0:1456b6f84c75 | 36 | }else{ |
yootee | 0:1456b6f84c75 | 37 | mode_ = 1; |
yootee | 0:1456b6f84c75 | 38 | } |
yootee | 0:1456b6f84c75 | 39 | } |
yootee | 0:1456b6f84c75 | 40 | |
yootee | 0:1456b6f84c75 | 41 | |
yootee | 0:1456b6f84c75 | 42 | void RotaryInc::specinit(PinName pinA, PinName pinB,double circumference,int resolution,int mode){ |
yootee | 0:1456b6f84c75 | 43 | //:mode_(mode),resolution_(resolution),circumference_(circumference){ |
yootee | 0:1456b6f84c75 | 44 | mode_ = mode; |
yootee | 0:1456b6f84c75 | 45 | resolution_ = resolution; |
yootee | 0:1456b6f84c75 | 46 | circumference_ = circumference; |
yootee | 0:1456b6f84c75 | 47 | reset(); |
yootee | 0:1456b6f84c75 | 48 | pin_a_ = new InterruptIn(pinA,PullUp); |
yootee | 0:1456b6f84c75 | 49 | pin_b_ = new InterruptIn(pinB,PullUp); |
yootee | 0:1456b6f84c75 | 50 | pin_a_->rise(callback(this,&RotaryInc::riseA)); |
yootee | 0:1456b6f84c75 | 51 | |
yootee | 0:1456b6f84c75 | 52 | if(mode_ == 2){ |
yootee | 0:1456b6f84c75 | 53 | pin_b_->rise(callback(this,&RotaryInc::riseB)); |
yootee | 0:1456b6f84c75 | 54 | }else if(mode_ == 4){ |
yootee | 0:1456b6f84c75 | 55 | pin_a_->fall(callback(this,&RotaryInc::fallA)); |
yootee | 0:1456b6f84c75 | 56 | pin_b_->rise(callback(this,&RotaryInc::riseB)); |
yootee | 0:1456b6f84c75 | 57 | pin_b_->fall(callback(this,&RotaryInc::fallB)); |
yootee | 0:1456b6f84c75 | 58 | }else{ |
yootee | 0:1456b6f84c75 | 59 | mode_ = 1; |
yootee | 0:1456b6f84c75 | 60 | } |
yootee | 0:1456b6f84c75 | 61 | } |
yootee | 0:1456b6f84c75 | 62 | |
yootee | 0:1456b6f84c75 | 63 | void RotaryInc::zero(){ |
yootee | 0:1456b6f84c75 | 64 | timer_.stop(); |
yootee | 0:1456b6f84c75 | 65 | timer_.reset(); |
yootee | 0:1456b6f84c75 | 66 | start_frag_ = false; |
yootee | 0:1456b6f84c75 | 67 | flag_ = false; |
yootee | 0:1456b6f84c75 | 68 | last_[0] = pulse_; |
yootee | 0:1456b6f84c75 | 69 | speed_ = 0; |
yootee | 0:1456b6f84c75 | 70 | count_ = 0; |
yootee | 0:1456b6f84c75 | 71 | sum_ = 0; |
yootee | 0:1456b6f84c75 | 72 | now_ = 0; |
yootee | 0:1456b6f84c75 | 73 | } |
yootee | 0:1456b6f84c75 | 74 | |
yootee | 0:1456b6f84c75 | 75 | void RotaryInc::calcu(){ |
yootee | 0:1456b6f84c75 | 76 | if(!start_frag_){ |
yootee | 0:1456b6f84c75 | 77 | timer_.start(); |
yootee | 0:1456b6f84c75 | 78 | start_frag_ = true; |
yootee | 0:1456b6f84c75 | 79 | last_[0] = pulse_; |
yootee | 0:1456b6f84c75 | 80 | pre_t_[0] = 0; |
yootee | 0:1456b6f84c75 | 81 | count_ = 1; |
yootee | 0:1456b6f84c75 | 82 | }else if(flag_){ |
yootee | 0:1456b6f84c75 | 83 | now_ = timer_.read(); |
yootee | 0:1456b6f84c75 | 84 | timer_.reset(); |
yootee | 0:1456b6f84c75 | 85 | sum_ -= pre_t_[count_]; |
yootee | 0:1456b6f84c75 | 86 | pre_t_[count_] = now_; |
yootee | 0:1456b6f84c75 | 87 | sum_ += now_; |
yootee | 0:1456b6f84c75 | 88 | speed_ = (double)(pulse_ - last_[count_]) / sum_; |
yootee | 0:1456b6f84c75 | 89 | last_[count_] = pulse_; |
yootee | 0:1456b6f84c75 | 90 | if(count_ < 19){ |
yootee | 0:1456b6f84c75 | 91 | count_++; |
yootee | 0:1456b6f84c75 | 92 | }else{ |
yootee | 0:1456b6f84c75 | 93 | count_ = 0; |
yootee | 0:1456b6f84c75 | 94 | } |
yootee | 0:1456b6f84c75 | 95 | }else{ |
yootee | 0:1456b6f84c75 | 96 | now_ = timer_.read(); |
yootee | 0:1456b6f84c75 | 97 | timer_.reset(); |
yootee | 0:1456b6f84c75 | 98 | pre_t_[count_] = now_; |
yootee | 0:1456b6f84c75 | 99 | sum_ += now_; |
yootee | 0:1456b6f84c75 | 100 | speed_ = (double)(pulse_ - last_[0]) / sum_; |
yootee | 0:1456b6f84c75 | 101 | last_[count_] = pulse_; |
yootee | 0:1456b6f84c75 | 102 | count_++; |
yootee | 0:1456b6f84c75 | 103 | if(count_ > 19){ |
yootee | 0:1456b6f84c75 | 104 | count_ = 0; |
yootee | 0:1456b6f84c75 | 105 | flag_ = true; |
yootee | 0:1456b6f84c75 | 106 | } |
yootee | 0:1456b6f84c75 | 107 | } |
yootee | 0:1456b6f84c75 | 108 | } |
yootee | 0:1456b6f84c75 | 109 | |
yootee | 0:1456b6f84c75 | 110 | void RotaryInc::riseA(){ |
yootee | 0:1456b6f84c75 | 111 | pin_b_->read() ? pulse_-- : pulse_++; |
yootee | 0:1456b6f84c75 | 112 | if(measur_){ |
yootee | 0:1456b6f84c75 | 113 | calcu(); |
yootee | 0:1456b6f84c75 | 114 | } |
yootee | 0:1456b6f84c75 | 115 | } |
yootee | 0:1456b6f84c75 | 116 | |
yootee | 0:1456b6f84c75 | 117 | void RotaryInc::fallA(){ |
yootee | 0:1456b6f84c75 | 118 | pin_b_->read() ? pulse_++ : pulse_--; |
yootee | 0:1456b6f84c75 | 119 | if(measur_){ |
yootee | 0:1456b6f84c75 | 120 | calcu(); |
yootee | 0:1456b6f84c75 | 121 | } |
yootee | 0:1456b6f84c75 | 122 | } |
yootee | 0:1456b6f84c75 | 123 | |
yootee | 0:1456b6f84c75 | 124 | void RotaryInc::riseB(){ |
yootee | 0:1456b6f84c75 | 125 | pin_a_->read() ? pulse_++ : pulse_--; |
yootee | 0:1456b6f84c75 | 126 | if(measur_){ |
yootee | 0:1456b6f84c75 | 127 | calcu(); |
yootee | 0:1456b6f84c75 | 128 | } |
yootee | 0:1456b6f84c75 | 129 | } |
yootee | 0:1456b6f84c75 | 130 | |
yootee | 0:1456b6f84c75 | 131 | void RotaryInc::fallB(){ |
yootee | 0:1456b6f84c75 | 132 | pin_a_->read() ? pulse_-- : pulse_++; |
yootee | 0:1456b6f84c75 | 133 | if(measur_){ |
yootee | 0:1456b6f84c75 | 134 | calcu(); |
yootee | 0:1456b6f84c75 | 135 | } |
yootee | 0:1456b6f84c75 | 136 | } |
yootee | 0:1456b6f84c75 | 137 | |
yootee | 0:1456b6f84c75 | 138 | long long RotaryInc::get(){ |
yootee | 0:1456b6f84c75 | 139 | return pulse_; |
yootee | 0:1456b6f84c75 | 140 | } |
yootee | 0:1456b6f84c75 | 141 | |
yootee | 0:1456b6f84c75 | 142 | double RotaryInc::getSpeed(){ |
yootee | 0:1456b6f84c75 | 143 | if(!measur_){ |
yootee | 0:1456b6f84c75 | 144 | return 0; |
yootee | 0:1456b6f84c75 | 145 | } |
yootee | 0:1456b6f84c75 | 146 | if(timer_.read_ms() > 150){ |
yootee | 0:1456b6f84c75 | 147 | zero(); |
yootee | 0:1456b6f84c75 | 148 | } |
yootee | 0:1456b6f84c75 | 149 | return speed_ / resolution_ / mode_ * circumference_; |
yootee | 0:1456b6f84c75 | 150 | } |
yootee | 0:1456b6f84c75 | 151 | |
yootee | 0:1456b6f84c75 | 152 | int RotaryInc::diff(){ |
yootee | 0:1456b6f84c75 | 153 | int diff = pulse_ - prev_; |
yootee | 0:1456b6f84c75 | 154 | prev_ = pulse_; |
yootee | 0:1456b6f84c75 | 155 | return diff; |
yootee | 0:1456b6f84c75 | 156 | } |
yootee | 0:1456b6f84c75 | 157 | |
yootee | 0:1456b6f84c75 | 158 | void RotaryInc::reset(){ |
yootee | 0:1456b6f84c75 | 159 | pulse_ = 0; |
yootee | 0:1456b6f84c75 | 160 | prev_ = 0; |
yootee | 0:1456b6f84c75 | 161 | if(measur_){ |
yootee | 0:1456b6f84c75 | 162 | zero(); |
yootee | 0:1456b6f84c75 | 163 | } |
yootee | 0:1456b6f84c75 | 164 | } |
yootee | 0:1456b6f84c75 | 165 | |
yootee | 0:1456b6f84c75 | 166 | RotaryInc::~RotaryInc(){ |
yootee | 0:1456b6f84c75 | 167 | pin_a_->disable_irq(); |
yootee | 0:1456b6f84c75 | 168 | pin_b_->disable_irq(); |
yootee | 0:1456b6f84c75 | 169 | delete pin_a_; |
yootee | 0:1456b6f84c75 | 170 | delete pin_b_; |
yootee | 0:1456b6f84c75 | 171 | } |