yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

sensor/rotary_inc_sp.cpp@2:e7b09385d197, 2022-03-27 (annotated)

Committer:
yootee
Date:
Sun Mar 27 04:39:16 2022 +0000
Revision:
2:e7b09385d197
Parent:
rotary_inc_sp.cpp@0:1456b6f84c75
Child:
22:394337a4205a
arrc?

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 0:1456b6f84c75 14 RotaryInc::RotaryInc(){
yootee 0:1456b6f84c75 15 measur_ = true;
yootee 0:1456b6f84c75 16 };
yootee 0:1456b6f84c75 17
yootee 0:1456b6f84c75 18 void RotaryInc::init(PinName pinA,PinName pinB){
yootee 0:1456b6f84c75 19 reset();
yootee 0:1456b6f84c75 20 pin_a_ = new InterruptIn(pinA,PullUp);
yootee 0:1456b6f84c75 21 pin_b_ = new InterruptIn(pinB,PullUp);
yootee 0:1456b6f84c75 22 pin_a_->rise(callback(this,&RotaryInc::riseA));
yootee 0:1456b6f84c75 23
yootee 0:1456b6f84c75 24 if(mode_ == 2){
yootee 0:1456b6f84c75 25 pin_b_->rise(callback(this,&RotaryInc::riseB));
yootee 0:1456b6f84c75 26 }else if(mode_ == 4){
yootee 0:1456b6f84c75 27 pin_a_->fall(callback(this,&RotaryInc::fallA));
yootee 0:1456b6f84c75 28 pin_b_->rise(callback(this,&RotaryInc::riseB));
yootee 0:1456b6f84c75 29 pin_b_->fall(callback(this,&RotaryInc::fallB));
yootee 0:1456b6f84c75 30 }else{
yootee 0:1456b6f84c75 31 mode_ = 1;
yootee 0:1456b6f84c75 32 }
yootee 0:1456b6f84c75 33 }
yootee 0:1456b6f84c75 34
yootee 0:1456b6f84c75 35
yootee 0:1456b6f84c75 36 void RotaryInc::specinit(PinName pinA, PinName pinB,double circumference,int resolution,int mode){
yootee 0:1456b6f84c75 37 //:mode_(mode),resolution_(resolution),circumference_(circumference){
yootee 0:1456b6f84c75 38 mode_ = mode;
yootee 0:1456b6f84c75 39 resolution_ = resolution;
yootee 0:1456b6f84c75 40 circumference_ = circumference;
yootee 0:1456b6f84c75 41 reset();
yootee 0:1456b6f84c75 42 pin_a_ = new InterruptIn(pinA,PullUp);
yootee 0:1456b6f84c75 43 pin_b_ = new InterruptIn(pinB,PullUp);
yootee 0:1456b6f84c75 44 pin_a_->rise(callback(this,&RotaryInc::riseA));
yootee 0:1456b6f84c75 45
yootee 0:1456b6f84c75 46 if(mode_ == 2){
yootee 0:1456b6f84c75 47 pin_b_->rise(callback(this,&RotaryInc::riseB));
yootee 0:1456b6f84c75 48 }else if(mode_ == 4){
yootee 0:1456b6f84c75 49 pin_a_->fall(callback(this,&RotaryInc::fallA));
yootee 0:1456b6f84c75 50 pin_b_->rise(callback(this,&RotaryInc::riseB));
yootee 0:1456b6f84c75 51 pin_b_->fall(callback(this,&RotaryInc::fallB));
yootee 0:1456b6f84c75 52 }else{
yootee 0:1456b6f84c75 53 mode_ = 1;
yootee 0:1456b6f84c75 54 }
yootee 0:1456b6f84c75 55 }
yootee 0:1456b6f84c75 56
yootee 0:1456b6f84c75 57 void RotaryInc::zero(){
yootee 0:1456b6f84c75 58 timer_.stop();
yootee 0:1456b6f84c75 59 timer_.reset();
yootee 0:1456b6f84c75 60 start_frag_ = false;
yootee 0:1456b6f84c75 61 flag_ = false;
yootee 0:1456b6f84c75 62 last_[0] = pulse_;
yootee 0:1456b6f84c75 63 speed_ = 0;
yootee 0:1456b6f84c75 64 count_ = 0;
yootee 0:1456b6f84c75 65 sum_ = 0;
yootee 0:1456b6f84c75 66 now_ = 0;
yootee 0:1456b6f84c75 67 }
yootee 0:1456b6f84c75 68
yootee 0:1456b6f84c75 69 void RotaryInc::calcu(){
yootee 0:1456b6f84c75 70 if(!start_frag_){
yootee 0:1456b6f84c75 71 timer_.start();
yootee 0:1456b6f84c75 72 start_frag_ = true;
yootee 0:1456b6f84c75 73 last_[0] = pulse_;
yootee 0:1456b6f84c75 74 pre_t_[0] = 0;
yootee 0:1456b6f84c75 75 count_ = 1;
yootee 0:1456b6f84c75 76 }else if(flag_){
yootee 0:1456b6f84c75 77 now_ = timer_.read();
yootee 0:1456b6f84c75 78 timer_.reset();
yootee 0:1456b6f84c75 79 sum_ -= pre_t_[count_];
yootee 0:1456b6f84c75 80 pre_t_[count_] = now_;
yootee 0:1456b6f84c75 81 sum_ += now_;
yootee 0:1456b6f84c75 82 speed_ = (double)(pulse_ - last_[count_]) / sum_;
yootee 0:1456b6f84c75 83 last_[count_] = pulse_;
yootee 0:1456b6f84c75 84 if(count_ < 19){
yootee 0:1456b6f84c75 85 count_++;
yootee 0:1456b6f84c75 86 }else{
yootee 0:1456b6f84c75 87 count_ = 0;
yootee 0:1456b6f84c75 88 }
yootee 0:1456b6f84c75 89 }else{
yootee 0:1456b6f84c75 90 now_ = timer_.read();
yootee 0:1456b6f84c75 91 timer_.reset();
yootee 0:1456b6f84c75 92 pre_t_[count_] = now_;
yootee 0:1456b6f84c75 93 sum_ += now_;
yootee 0:1456b6f84c75 94 speed_ = (double)(pulse_ - last_[0]) / sum_;
yootee 0:1456b6f84c75 95 last_[count_] = pulse_;
yootee 0:1456b6f84c75 96 count_++;
yootee 0:1456b6f84c75 97 if(count_ > 19){
yootee 0:1456b6f84c75 98 count_ = 0;
yootee 0:1456b6f84c75 99 flag_ = true;
yootee 0:1456b6f84c75 100 }
yootee 0:1456b6f84c75 101 }
yootee 0:1456b6f84c75 102 }
yootee 0:1456b6f84c75 103
yootee 0:1456b6f84c75 104 void RotaryInc::riseA(){
yootee 0:1456b6f84c75 105 pin_b_->read() ? pulse_-- : pulse_++;
yootee 0:1456b6f84c75 106 if(measur_){
yootee 0:1456b6f84c75 107 calcu();
yootee 0:1456b6f84c75 108 }
yootee 0:1456b6f84c75 109 }
yootee 0:1456b6f84c75 110
yootee 0:1456b6f84c75 111 void RotaryInc::fallA(){
yootee 0:1456b6f84c75 112 pin_b_->read() ? pulse_++ : pulse_--;
yootee 0:1456b6f84c75 113 if(measur_){
yootee 0:1456b6f84c75 114 calcu();
yootee 0:1456b6f84c75 115 }
yootee 0:1456b6f84c75 116 }
yootee 0:1456b6f84c75 117
yootee 0:1456b6f84c75 118 void RotaryInc::riseB(){
yootee 0:1456b6f84c75 119 pin_a_->read() ? pulse_++ : pulse_--;
yootee 0:1456b6f84c75 120 if(measur_){
yootee 0:1456b6f84c75 121 calcu();
yootee 0:1456b6f84c75 122 }
yootee 0:1456b6f84c75 123 }
yootee 0:1456b6f84c75 124
yootee 0:1456b6f84c75 125 void RotaryInc::fallB(){
yootee 0:1456b6f84c75 126 pin_a_->read() ? pulse_-- : pulse_++;
yootee 0:1456b6f84c75 127 if(measur_){
yootee 0:1456b6f84c75 128 calcu();
yootee 0:1456b6f84c75 129 }
yootee 0:1456b6f84c75 130 }
yootee 0:1456b6f84c75 131
yootee 0:1456b6f84c75 132 long long RotaryInc::get(){
yootee 0:1456b6f84c75 133 return pulse_;
yootee 0:1456b6f84c75 134 }
yootee 0:1456b6f84c75 135
yootee 0:1456b6f84c75 136 double RotaryInc::getSpeed(){
yootee 0:1456b6f84c75 137 if(!measur_){
yootee 0:1456b6f84c75 138 return 0;
yootee 0:1456b6f84c75 139 }
yootee 0:1456b6f84c75 140 if(timer_.read_ms() > 150){
yootee 0:1456b6f84c75 141 zero();
yootee 0:1456b6f84c75 142 }
yootee 0:1456b6f84c75 143 return speed_ / resolution_ / mode_ * circumference_;
yootee 0:1456b6f84c75 144 }
yootee 0:1456b6f84c75 145
yootee 0:1456b6f84c75 146 int RotaryInc::diff(){
yootee 0:1456b6f84c75 147 int diff = pulse_ - prev_;
yootee 0:1456b6f84c75 148 prev_ = pulse_;
yootee 0:1456b6f84c75 149 return diff;
yootee 0:1456b6f84c75 150 }
yootee 0:1456b6f84c75 151
yootee 0:1456b6f84c75 152 void RotaryInc::reset(){
yootee 0:1456b6f84c75 153 pulse_ = 0;
yootee 0:1456b6f84c75 154 prev_ = 0;
yootee 0:1456b6f84c75 155 if(measur_){
yootee 0:1456b6f84c75 156 zero();
yootee 0:1456b6f84c75 157 }
yootee 0:1456b6f84c75 158 }
yootee 0:1456b6f84c75 159
yootee 0:1456b6f84c75 160 RotaryInc::~RotaryInc(){
yootee 0:1456b6f84c75 161 pin_a_->disable_irq();
yootee 0:1456b6f84c75 162 pin_b_->disable_irq();
yootee 0:1456b6f84c75 163 delete pin_a_;
yootee 0:1456b6f84c75 164 delete pin_b_;
yootee 0:1456b6f84c75 165 }