miki sumito / rotaryinc

現在値を取得できるライブラリ

rotaryinc.cpp@0:e05a40ae7bec, 2022-08-24 (annotated)

Committer:
m_smt
Date:
Wed Aug 24 06:50:01 2022 +0000
Revision:
0:e05a40ae7bec
nowvalue get

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m_smt 0:e05a40ae7bec 1 #include "rotaryinc.hpp"
m_smt 0:e05a40ae7bec 2
m_smt 0:e05a40ae7bec 3 rotaryinc::rotaryinc(PinName pinA,PinName pinB,double diameter,double resolution,double dt)
m_smt 0:e05a40ae7bec 4 {
m_smt 0:e05a40ae7bec 5 Diameter = diameter;
m_smt 0:e05a40ae7bec 6 Resolution = resolution;
m_smt 0:e05a40ae7bec 7 dt_ = dt;
m_smt 0:e05a40ae7bec 8 init(pinA,pinB);
m_smt 0:e05a40ae7bec 9 pulse[0] = 0;
m_smt 0:e05a40ae7bec 10 pulse[1] = 0;
m_smt 0:e05a40ae7bec 11 speed = 0;
m_smt 0:e05a40ae7bec 12 timer.start();
m_smt 0:e05a40ae7bec 13 }
m_smt 0:e05a40ae7bec 14
m_smt 0:e05a40ae7bec 15 void rotaryinc::init(PinName pin_a,PinName pin_b)
m_smt 0:e05a40ae7bec 16 {
m_smt 0:e05a40ae7bec 17 pin_a_ = new InterruptIn(pin_a,PullUp);
m_smt 0:e05a40ae7bec 18 pin_b_ = new InterruptIn(pin_b,PullUp);
m_smt 0:e05a40ae7bec 19 pin_a_->rise(callback(this,&rotaryinc::riseA));
m_smt 0:e05a40ae7bec 20 pin_a_->fall(callback(this,&rotaryinc::fallA));
m_smt 0:e05a40ae7bec 21 pin_b_->rise(callback(this,&rotaryinc::riseB));
m_smt 0:e05a40ae7bec 22 pin_b_->fall(callback(this,&rotaryinc::fallB));
m_smt 0:e05a40ae7bec 23 }
m_smt 0:e05a40ae7bec 24
m_smt 0:e05a40ae7bec 25 void rotaryinc::riseA()
m_smt 0:e05a40ae7bec 26 {
m_smt 0:e05a40ae7bec 27 pin_b_ -> read() ? pulse[1]-- : pulse[1]++;
m_smt 0:e05a40ae7bec 28 }
m_smt 0:e05a40ae7bec 29
m_smt 0:e05a40ae7bec 30 void rotaryinc::fallA()
m_smt 0:e05a40ae7bec 31 {
m_smt 0:e05a40ae7bec 32 pin_b_ -> read() ? pulse[1]++ : pulse[1]--;
m_smt 0:e05a40ae7bec 33 }
m_smt 0:e05a40ae7bec 34
m_smt 0:e05a40ae7bec 35 void rotaryinc::riseB()
m_smt 0:e05a40ae7bec 36 {
m_smt 0:e05a40ae7bec 37 pin_a_ -> read() ? pulse[1]++ : pulse[1]--;
m_smt 0:e05a40ae7bec 38 }
m_smt 0:e05a40ae7bec 39
m_smt 0:e05a40ae7bec 40 void rotaryinc::fallB()
m_smt 0:e05a40ae7bec 41 {
m_smt 0:e05a40ae7bec 42 pin_a_ -> read() ? pulse[1]-- : pulse[1]++;
m_smt 0:e05a40ae7bec 43 }
m_smt 0:e05a40ae7bec 44
m_smt 0:e05a40ae7bec 45 double rotaryinc::getpulse()
m_smt 0:e05a40ae7bec 46 {
m_smt 0:e05a40ae7bec 47 return pulse[1]/4;
m_smt 0:e05a40ae7bec 48 }
m_smt 0:e05a40ae7bec 49
m_smt 0:e05a40ae7bec 50 double rotaryinc::getspeed()
m_smt 0:e05a40ae7bec 51 {
m_smt 0:e05a40ae7bec 52 speed = (M_PI * Diameter) * (pulse[1]/4 - pulse[0]) / Resolution / dt_;
m_smt 0:e05a40ae7bec 53 pulse[0] = pulse[1]/4;
m_smt 0:e05a40ae7bec 54 return speed;
m_smt 0:e05a40ae7bec 55 }
m_smt 0:e05a40ae7bec 56
m_smt 0:e05a40ae7bec 57 double rotaryinc::gettheta()
m_smt 0:e05a40ae7bec 58 {
m_smt 0:e05a40ae7bec 59 theta[1] = pulse[1]/4 / Resolution * 360;
m_smt 0:e05a40ae7bec 60 return theta[1];
m_smt 0:e05a40ae7bec 61 }
m_smt 0:e05a40ae7bec 62
m_smt 0:e05a40ae7bec 63 double rotaryinc::getomega()
m_smt 0:e05a40ae7bec 64 {
m_smt 0:e05a40ae7bec 65 omega = (theta[1] - theta[0]) / dt_;
m_smt 0:e05a40ae7bec 66 theta[0] = theta[1];
m_smt 0:e05a40ae7bec 67 return omega;
m_smt 0:e05a40ae7bec 68 }
m_smt 0:e05a40ae7bec 69
m_smt 0:e05a40ae7bec 70 double rotaryinc::getrevolution()
m_smt 0:e05a40ae7bec 71 {
m_smt 0:e05a40ae7bec 72 return pulse[1]/4 / Resolution;
m_smt 0:e05a40ae7bec 73 }
m_smt 0:e05a40ae7bec 74
m_smt 0:e05a40ae7bec 75 void rotaryinc::loop()
m_smt 0:e05a40ae7bec 76 {
m_smt 0:e05a40ae7bec 77 time = timer.read();
m_smt 0:e05a40ae7bec 78 while(timer.read() - time <= dt_);
m_smt 0:e05a40ae7bec 79 }
m_smt 0:e05a40ae7bec 80
m_smt 0:e05a40ae7bec 81 rotaryinc::~rotaryinc(){
m_smt 0:e05a40ae7bec 82 pin_a_->disable_irq();
m_smt 0:e05a40ae7bec 83 pin_b_->disable_irq();
m_smt 0:e05a40ae7bec 84 delete pin_a_;
m_smt 0:e05a40ae7bec 85 delete pin_b_;
m_smt 0:e05a40ae7bec 86 }