yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }