![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
my new gear...
sensor/rotary_inc_sp.cpp
- Committer:
- yootee
- Date:
- 22 months ago
- Revision:
- 22:394337a4205a
- Parent:
- 2:e7b09385d197
File content as of revision 22:394337a4205a:
#include "rotary_inc_sp.hpp" RotaryInc::RotaryInc(PinName pin_a, PinName pin_b,int mode):mode_(mode){ measur_ = false; init(pin_a,pin_b); } RotaryInc::RotaryInc(PinName pin_a,PinName pin_b,double circumference,int resolution,int mode) :mode_(mode),resolution_(resolution),circumference_(circumference){ measur_ = true; init(pin_a,pin_b); } RotaryInc::RotaryInc(Port port,double circumference,int resolution,int mode) :mode_(mode),resolution_(resolution),circumference_(circumference){ measur_ = true; init(port.pin[0],port.pin[1]); } RotaryInc::RotaryInc(){ measur_ = true; }; void RotaryInc::init(PinName pinA,PinName pinB){ reset(); pin_a_ = new InterruptIn(pinA,PullUp); pin_b_ = new InterruptIn(pinB,PullUp); pin_a_->rise(callback(this,&RotaryInc::riseA)); if(mode_ == 2){ pin_b_->rise(callback(this,&RotaryInc::riseB)); }else if(mode_ == 4){ pin_a_->fall(callback(this,&RotaryInc::fallA)); pin_b_->rise(callback(this,&RotaryInc::riseB)); pin_b_->fall(callback(this,&RotaryInc::fallB)); }else{ mode_ = 1; } } void RotaryInc::specinit(PinName pinA, PinName pinB,double circumference,int resolution,int mode){ //:mode_(mode),resolution_(resolution),circumference_(circumference){ mode_ = mode; resolution_ = resolution; circumference_ = circumference; reset(); pin_a_ = new InterruptIn(pinA,PullUp); pin_b_ = new InterruptIn(pinB,PullUp); pin_a_->rise(callback(this,&RotaryInc::riseA)); if(mode_ == 2){ pin_b_->rise(callback(this,&RotaryInc::riseB)); }else if(mode_ == 4){ pin_a_->fall(callback(this,&RotaryInc::fallA)); pin_b_->rise(callback(this,&RotaryInc::riseB)); pin_b_->fall(callback(this,&RotaryInc::fallB)); }else{ mode_ = 1; } } void RotaryInc::zero(){ timer_.stop(); timer_.reset(); start_frag_ = false; flag_ = false; last_[0] = pulse_; speed_ = 0; count_ = 0; sum_ = 0; now_ = 0; } void RotaryInc::calcu(){ if(!start_frag_){ timer_.start(); start_frag_ = true; last_[0] = pulse_; pre_t_[0] = 0; count_ = 1; }else if(flag_){ now_ = timer_.read(); timer_.reset(); sum_ -= pre_t_[count_]; pre_t_[count_] = now_; sum_ += now_; speed_ = (double)(pulse_ - last_[count_]) / sum_; last_[count_] = pulse_; if(count_ < 19){ count_++; }else{ count_ = 0; } }else{ now_ = timer_.read(); timer_.reset(); pre_t_[count_] = now_; sum_ += now_; speed_ = (double)(pulse_ - last_[0]) / sum_; last_[count_] = pulse_; count_++; if(count_ > 19){ count_ = 0; flag_ = true; } } } void RotaryInc::riseA(){ pin_b_->read() ? pulse_-- : pulse_++; if(measur_){ calcu(); } } void RotaryInc::fallA(){ pin_b_->read() ? pulse_++ : pulse_--; if(measur_){ calcu(); } } void RotaryInc::riseB(){ pin_a_->read() ? pulse_++ : pulse_--; if(measur_){ calcu(); } } void RotaryInc::fallB(){ pin_a_->read() ? pulse_-- : pulse_++; if(measur_){ calcu(); } } long long RotaryInc::get(){ return pulse_; } double RotaryInc::getSpeed(){ if(!measur_){ return 0; } if(timer_.read_ms() > 150){ zero(); } return speed_ / resolution_ / mode_ * circumference_; } int RotaryInc::diff(){ int diff = pulse_ - prev_; prev_ = pulse_; return diff; } void RotaryInc::reset(){ pulse_ = 0; prev_ = 0; if(measur_){ zero(); } } RotaryInc::~RotaryInc(){ pin_a_->disable_irq(); pin_b_->disable_irq(); delete pin_a_; delete pin_b_; }