明石高専ロボ研 mbedライブラリ
Dependents: MDD_L432KC USB2RS485 pathtracking odometry ... more
Diff: rotary_inc.cpp
- Revision:
- 0:ca84ed7518f5
- Child:
- 3:28c77df7c0b6
diff -r 000000000000 -r ca84ed7518f5 rotary_inc.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rotary_inc.cpp Thu Jan 02 09:30:06 2020 +0000 @@ -0,0 +1,139 @@ +#include "rotary_inc.hpp" + +RotaryInc::RotaryInc(PinName pinA, PinName pinB,int mode):mode_(mode){ + measur_ = false; + init(pinA,pinB); +} + +RotaryInc::RotaryInc(PinName pinA,PinName pinB,double circumference,int resolution,int mode) + :mode_(mode),resolution_(resolution),circumference_(circumference){ + measur_ = true; + init(pinA,pinB); +} + +void RotaryInc::init(PinName pinA,PinName pinB){ + reset(); + A_ = new InterruptIn(pinA,PullUp); + B_ = new InterruptIn(pinB,PullUp); + A_->rise(callback(this,&RotaryInc::riseA)); + + if(mode_ == 2){ + A_->fall(callback(this,&RotaryInc::fallA)); + }else if(mode_ == 4){ + A_->fall(callback(this,&RotaryInc::fallA)); + B_->rise(callback(this,&RotaryInc::riseB)); + 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(){ + B_->read() ? pulse_-- : pulse_++; + if(measur_){ + calcu(); + } +} + +void RotaryInc::fallA(){ + B_->read() ? pulse_++ : pulse_--; + if(measur_){ + calcu(); + } +} + +void RotaryInc::riseB(){ + A_->read() ? pulse_++ : pulse_--; + if(measur_){ + calcu(); + } +} + +void RotaryInc::fallB(){ + 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(){ + A_->disable_irq(); + B_->disable_irq(); + delete A_; + delete B_; +} \ No newline at end of file