rotary_inc_sp.cpp

00001 #include "rotary_inc_sp.hpp"
00002 
00003 RotaryInc::RotaryInc(PinName pin_a, PinName pin_b,int mode):mode_(mode){
00004     measur_ = false;
00005     init(pin_a,pin_b);
00006 }
00007 
00008 RotaryInc::RotaryInc(PinName pin_a,PinName pin_b,double circumference,int resolution,int mode)
00009     :mode_(mode),resolution_(resolution),circumference_(circumference){
00010     measur_ = true;
00011     init(pin_a,pin_b);
00012 }
00013 
00014 RotaryInc::RotaryInc(Port port,double circumference,int resolution,int mode)
00015     :mode_(mode),resolution_(resolution),circumference_(circumference){
00016     measur_ = true;
00017     init(port.pin[0],port.pin[1]);
00018 }
00019 
00020 RotaryInc::RotaryInc(){
00021     measur_ = true;
00022 };
00023 
00024 void RotaryInc::init(PinName pinA,PinName pinB){
00025     reset();
00026     pin_a_ = new InterruptIn(pinA,PullUp);
00027     pin_b_ = new InterruptIn(pinB,PullUp);
00028     pin_a_->rise(callback(this,&RotaryInc::riseA));
00029         
00030     if(mode_ == 2){
00031         pin_b_->rise(callback(this,&RotaryInc::riseB));
00032     }else if(mode_ == 4){
00033         pin_a_->fall(callback(this,&RotaryInc::fallA));
00034         pin_b_->rise(callback(this,&RotaryInc::riseB));
00035         pin_b_->fall(callback(this,&RotaryInc::fallB));
00036     }else{
00037         mode_ = 1;
00038     }
00039 }
00040 
00041 
00042 void RotaryInc::specinit(PinName pinA, PinName pinB,double circumference,int resolution,int mode){
00043      //:mode_(mode),resolution_(resolution),circumference_(circumference){  
00044     mode_ = mode;
00045     resolution_ = resolution;
00046     circumference_ = circumference;
00047     reset();
00048     pin_a_ = new InterruptIn(pinA,PullUp);
00049     pin_b_ = new InterruptIn(pinB,PullUp);
00050     pin_a_->rise(callback(this,&RotaryInc::riseA));
00051         
00052     if(mode_ == 2){
00053         pin_b_->rise(callback(this,&RotaryInc::riseB));
00054     }else if(mode_ == 4){
00055         pin_a_->fall(callback(this,&RotaryInc::fallA));
00056         pin_b_->rise(callback(this,&RotaryInc::riseB));
00057         pin_b_->fall(callback(this,&RotaryInc::fallB));
00058     }else{
00059         mode_ = 1;
00060     }
00061 }
00062 
00063 void RotaryInc::zero(){
00064     timer_.stop();
00065     timer_.reset();
00066     start_frag_ = false;
00067     flag_ = false;
00068     last_[0] = pulse_;
00069     speed_ = 0;
00070     count_ = 0;
00071     sum_ = 0;
00072     now_ = 0;
00073 }
00074 
00075 void RotaryInc::calcu(){
00076     if(!start_frag_){
00077         timer_.start();
00078         start_frag_ = true;
00079         last_[0] = pulse_;
00080         pre_t_[0] = 0;
00081         count_ = 1;
00082     }else if(flag_){
00083         now_ = timer_.read();
00084         timer_.reset();
00085         sum_ -= pre_t_[count_];
00086         pre_t_[count_] = now_;
00087         sum_ += now_;
00088         speed_ = (double)(pulse_ - last_[count_]) / sum_;
00089         last_[count_] = pulse_;
00090         if(count_ < 19){
00091             count_++;
00092         }else{
00093             count_ = 0;
00094         }
00095     }else{
00096         now_ = timer_.read();
00097         timer_.reset();
00098         pre_t_[count_] = now_;
00099         sum_ += now_;
00100         speed_ = (double)(pulse_ - last_[0]) / sum_;
00101         last_[count_] = pulse_;
00102         count_++;
00103         if(count_ > 19){
00104             count_ = 0;
00105             flag_ = true;
00106         }
00107     }
00108 }
00109 
00110 void RotaryInc::riseA(){
00111     pin_b_->read() ? pulse_-- : pulse_++;
00112     if(measur_){
00113         calcu();
00114     }
00115 }
00116 
00117 void RotaryInc::fallA(){
00118     pin_b_->read() ? pulse_++ : pulse_--;
00119     if(measur_){
00120         calcu();
00121     }
00122 }
00123 
00124 void RotaryInc::riseB(){
00125     pin_a_->read() ? pulse_++ : pulse_--;
00126     if(measur_){
00127         calcu();
00128     }
00129 }
00130 
00131 void RotaryInc::fallB(){
00132     pin_a_->read() ? pulse_-- : pulse_++;
00133     if(measur_){
00134         calcu();
00135     }
00136 }
00137 
00138 long long RotaryInc::get(){
00139     return pulse_;
00140 }
00141 
00142 double RotaryInc::getSpeed(){
00143     if(!measur_){
00144         return 0;
00145     }
00146     if(timer_.read_ms() > 150){
00147         zero();
00148     }
00149     return speed_ / resolution_ / mode_ * circumference_;
00150 }
00151 
00152 int RotaryInc::diff(){
00153     int diff = pulse_ - prev_;
00154     prev_ = pulse_;
00155     return diff;
00156 }
00157     
00158 void RotaryInc::reset(){
00159     pulse_ = 0;
00160     prev_ = 0;
00161     if(measur_){
00162         zero();
00163     }
00164 }
00165 
00166 RotaryInc::~RotaryInc(){
00167     pin_a_->disable_irq();
00168     pin_b_->disable_irq();
00169     delete pin_a_;
00170     delete pin_b_;
00171 }