Saya Matsuura / Mbed 2 deprecated drum

明石高専ロボ研 drum

Dependencies:   mbed

rotary_inc.cpp@7:678c6b604ac7, 2020-11-12 (annotated)

Committer:
TanakaRobo
Date:
Thu Nov 12 16:44:10 2020 +0000
Revision:
7:678c6b604ac7
Parent:
3:28c77df7c0b6 
add lidar lite lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 0:ca84ed7518f5 1 #include "rotary_inc.hpp"
TanakaRobo 0:ca84ed7518f5 2
TanakaRobo 3:28c77df7c0b6 3 RotaryInc::RotaryInc(PinName pin_a, PinName pin_b,int mode):mode_(mode){
TanakaRobo 0:ca84ed7518f5 4 measur_ = false;
TanakaRobo 3:28c77df7c0b6 5 init(pin_a,pin_b);
TanakaRobo 0:ca84ed7518f5 6 }
TanakaRobo 0:ca84ed7518f5 7
TanakaRobo 3:28c77df7c0b6 8 RotaryInc::RotaryInc(PinName pin_a,PinName pin_b,double circumference,int resolution,int mode)
TanakaRobo 0:ca84ed7518f5 9 :mode_(mode),resolution_(resolution),circumference_(circumference){
TanakaRobo 0:ca84ed7518f5 10 measur_ = true;
TanakaRobo 3:28c77df7c0b6 11 init(pin_a,pin_b);
TanakaRobo 0:ca84ed7518f5 12 }
TanakaRobo 0:ca84ed7518f5 13
TanakaRobo 0:ca84ed7518f5 14 void RotaryInc::init(PinName pinA,PinName pinB){
TanakaRobo 0:ca84ed7518f5 15 reset();
TanakaRobo 3:28c77df7c0b6 16 pin_a_ = new InterruptIn(pinA,PullUp);
TanakaRobo 3:28c77df7c0b6 17 pin_b_ = new InterruptIn(pinB,PullUp);
TanakaRobo 3:28c77df7c0b6 18 pin_a_->rise(callback(this,&RotaryInc::riseA));
TanakaRobo 0:ca84ed7518f5 19
TanakaRobo 0:ca84ed7518f5 20 if(mode_ == 2){
TanakaRobo 3:28c77df7c0b6 21 pin_a_->fall(callback(this,&RotaryInc::fallA));
TanakaRobo 0:ca84ed7518f5 22 }else if(mode_ == 4){
TanakaRobo 3:28c77df7c0b6 23 pin_a_->fall(callback(this,&RotaryInc::fallA));
TanakaRobo 3:28c77df7c0b6 24 pin_b_->rise(callback(this,&RotaryInc::riseB));
TanakaRobo 3:28c77df7c0b6 25 pin_b_->fall(callback(this,&RotaryInc::fallB));
TanakaRobo 0:ca84ed7518f5 26 }else{
TanakaRobo 0:ca84ed7518f5 27 mode_ = 1;
TanakaRobo 0:ca84ed7518f5 28 }
TanakaRobo 0:ca84ed7518f5 29 }
TanakaRobo 0:ca84ed7518f5 30
TanakaRobo 0:ca84ed7518f5 31 void RotaryInc::zero(){
TanakaRobo 0:ca84ed7518f5 32 timer_.stop();
TanakaRobo 0:ca84ed7518f5 33 timer_.reset();
TanakaRobo 0:ca84ed7518f5 34 start_frag_ = false;
TanakaRobo 0:ca84ed7518f5 35 flag_ = false;
TanakaRobo 0:ca84ed7518f5 36 last_[0] = pulse_;
TanakaRobo 0:ca84ed7518f5 37 speed_ = 0;
TanakaRobo 0:ca84ed7518f5 38 count_ = 0;
TanakaRobo 0:ca84ed7518f5 39 sum_ = 0;
TanakaRobo 0:ca84ed7518f5 40 now_ = 0;
TanakaRobo 0:ca84ed7518f5 41 }
TanakaRobo 0:ca84ed7518f5 42
TanakaRobo 0:ca84ed7518f5 43 void RotaryInc::calcu(){
TanakaRobo 0:ca84ed7518f5 44 if(!start_frag_){
TanakaRobo 0:ca84ed7518f5 45 timer_.start();
TanakaRobo 0:ca84ed7518f5 46 start_frag_ = true;
TanakaRobo 0:ca84ed7518f5 47 last_[0] = pulse_;
TanakaRobo 0:ca84ed7518f5 48 pre_t_[0] = 0;
TanakaRobo 0:ca84ed7518f5 49 count_ = 1;
TanakaRobo 0:ca84ed7518f5 50 }else if(flag_){
TanakaRobo 0:ca84ed7518f5 51 now_ = timer_.read();
TanakaRobo 0:ca84ed7518f5 52 timer_.reset();
TanakaRobo 0:ca84ed7518f5 53 sum_ -= pre_t_[count_];
TanakaRobo 0:ca84ed7518f5 54 pre_t_[count_] = now_;
TanakaRobo 0:ca84ed7518f5 55 sum_ += now_;
TanakaRobo 0:ca84ed7518f5 56 speed_ = (double)(pulse_ - last_[count_]) / sum_;
TanakaRobo 0:ca84ed7518f5 57 last_[count_] = pulse_;
TanakaRobo 0:ca84ed7518f5 58 if(count_ < 19){
TanakaRobo 0:ca84ed7518f5 59 count_++;
TanakaRobo 0:ca84ed7518f5 60 }else{
TanakaRobo 0:ca84ed7518f5 61 count_ = 0;
TanakaRobo 0:ca84ed7518f5 62 }
TanakaRobo 0:ca84ed7518f5 63 }else{
TanakaRobo 0:ca84ed7518f5 64 now_ = timer_.read();
TanakaRobo 0:ca84ed7518f5 65 timer_.reset();
TanakaRobo 0:ca84ed7518f5 66 pre_t_[count_] = now_;
TanakaRobo 0:ca84ed7518f5 67 sum_ += now_;
TanakaRobo 0:ca84ed7518f5 68 speed_ = (double)(pulse_ - last_[0]) / sum_;
TanakaRobo 0:ca84ed7518f5 69 last_[count_] = pulse_;
TanakaRobo 0:ca84ed7518f5 70 count_++;
TanakaRobo 0:ca84ed7518f5 71 if(count_ > 19){
TanakaRobo 0:ca84ed7518f5 72 count_ = 0;
TanakaRobo 0:ca84ed7518f5 73 flag_ = true;
TanakaRobo 0:ca84ed7518f5 74 }
TanakaRobo 0:ca84ed7518f5 75 }
TanakaRobo 0:ca84ed7518f5 76 }
TanakaRobo 0:ca84ed7518f5 77
TanakaRobo 0:ca84ed7518f5 78 void RotaryInc::riseA(){
TanakaRobo 3:28c77df7c0b6 79 pin_b_->read() ? pulse_-- : pulse_++;
TanakaRobo 0:ca84ed7518f5 80 if(measur_){
TanakaRobo 0:ca84ed7518f5 81 calcu();
TanakaRobo 0:ca84ed7518f5 82 }
TanakaRobo 0:ca84ed7518f5 83 }
TanakaRobo 0:ca84ed7518f5 84
TanakaRobo 0:ca84ed7518f5 85 void RotaryInc::fallA(){
TanakaRobo 3:28c77df7c0b6 86 pin_b_->read() ? pulse_++ : pulse_--;
TanakaRobo 0:ca84ed7518f5 87 if(measur_){
TanakaRobo 0:ca84ed7518f5 88 calcu();
TanakaRobo 0:ca84ed7518f5 89 }
TanakaRobo 0:ca84ed7518f5 90 }
TanakaRobo 0:ca84ed7518f5 91
TanakaRobo 0:ca84ed7518f5 92 void RotaryInc::riseB(){
TanakaRobo 3:28c77df7c0b6 93 pin_a_->read() ? pulse_++ : pulse_--;
TanakaRobo 0:ca84ed7518f5 94 if(measur_){
TanakaRobo 0:ca84ed7518f5 95 calcu();
TanakaRobo 0:ca84ed7518f5 96 }
TanakaRobo 0:ca84ed7518f5 97 }
TanakaRobo 0:ca84ed7518f5 98
TanakaRobo 0:ca84ed7518f5 99 void RotaryInc::fallB(){
TanakaRobo 3:28c77df7c0b6 100 pin_a_->read() ? pulse_-- : pulse_++;
TanakaRobo 0:ca84ed7518f5 101 if(measur_){
TanakaRobo 0:ca84ed7518f5 102 calcu();
TanakaRobo 0:ca84ed7518f5 103 }
TanakaRobo 0:ca84ed7518f5 104 }
TanakaRobo 0:ca84ed7518f5 105
TanakaRobo 0:ca84ed7518f5 106 long long RotaryInc::get(){
TanakaRobo 0:ca84ed7518f5 107 return pulse_;
TanakaRobo 0:ca84ed7518f5 108 }
TanakaRobo 0:ca84ed7518f5 109
TanakaRobo 0:ca84ed7518f5 110 double RotaryInc::getSpeed(){
TanakaRobo 0:ca84ed7518f5 111 if(!measur_){
TanakaRobo 0:ca84ed7518f5 112 return 0;
TanakaRobo 0:ca84ed7518f5 113 }
TanakaRobo 0:ca84ed7518f5 114 if(timer_.read_ms() > 150){
TanakaRobo 0:ca84ed7518f5 115 zero();
TanakaRobo 0:ca84ed7518f5 116 }
TanakaRobo 0:ca84ed7518f5 117 return speed_ / resolution_ / mode_ * circumference_;
TanakaRobo 0:ca84ed7518f5 118 }
TanakaRobo 0:ca84ed7518f5 119
TanakaRobo 0:ca84ed7518f5 120 int RotaryInc::diff(){
TanakaRobo 0:ca84ed7518f5 121 int diff = pulse_ - prev_;
TanakaRobo 0:ca84ed7518f5 122 prev_ = pulse_;
TanakaRobo 0:ca84ed7518f5 123 return diff;
TanakaRobo 0:ca84ed7518f5 124 }
TanakaRobo 0:ca84ed7518f5 125
TanakaRobo 0:ca84ed7518f5 126 void RotaryInc::reset(){
TanakaRobo 0:ca84ed7518f5 127 pulse_ = 0;
TanakaRobo 0:ca84ed7518f5 128 prev_ = 0;
TanakaRobo 0:ca84ed7518f5 129 if(measur_){
TanakaRobo 0:ca84ed7518f5 130 zero();
TanakaRobo 0:ca84ed7518f5 131 }
TanakaRobo 0:ca84ed7518f5 132 }
TanakaRobo 0:ca84ed7518f5 133
TanakaRobo 0:ca84ed7518f5 134 RotaryInc::~RotaryInc(){
TanakaRobo 3:28c77df7c0b6 135 pin_a_->disable_irq();
TanakaRobo 3:28c77df7c0b6 136 pin_b_->disable_irq();
TanakaRobo 3:28c77df7c0b6 137 delete pin_a_;
TanakaRobo 3:28c77df7c0b6 138 delete pin_b_;
TanakaRobo 0:ca84ed7518f5 139 }