![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
明石高専ロボ研 mbedライブラリ
Dependencies: mbed
rotary_inc.cpp
- Committer:
- TanakaRobo
- Date:
- 2020-01-06
- Revision:
- 3:28c77df7c0b6
- Parent:
- 0:ca84ed7518f5
File content as of revision 3:28c77df7c0b6:
#include "rotary_inc.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); } 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_a_->fall(callback(this,&RotaryInc::fallA)); }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_; }