Hiroki Tanaka / Mbed 2 deprecated L432KC_temp

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

library/RotaryInc.cpp@1:17051435cfc5, 2019-07-09 (annotated)

Committer:
TanakaRobo
Date:
Tue Jul 09 07:56:07 2019 +0000
Revision:
1:17051435cfc5
Parent:
0:a8a56075e947 
first commit

Who changed what in which revision?

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