現在値を取得できるライブラリ
Revision 0:e05a40ae7bec, committed 2022-08-24
- Comitter:
- m_smt
- Date:
- Wed Aug 24 06:50:01 2022 +0000
- Commit message:
- nowvalue get
Changed in this revision
rotaryinc.cpp | Show annotated file Show diff for this revision Revisions of this file |
rotaryinc.hpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rotaryinc.cpp Wed Aug 24 06:50:01 2022 +0000 @@ -0,0 +1,86 @@ +#include "rotaryinc.hpp" + +rotaryinc::rotaryinc(PinName pinA,PinName pinB,double diameter,double resolution,double dt) +{ + Diameter = diameter; + Resolution = resolution; + dt_ = dt; + init(pinA,pinB); + pulse[0] = 0; + pulse[1] = 0; + speed = 0; + timer.start(); +} + +void rotaryinc::init(PinName pin_a,PinName pin_b) +{ + pin_a_ = new InterruptIn(pin_a,PullUp); + pin_b_ = new InterruptIn(pin_b,PullUp); + pin_a_->rise(callback(this,&rotaryinc::riseA)); + pin_a_->fall(callback(this,&rotaryinc::fallA)); + pin_b_->rise(callback(this,&rotaryinc::riseB)); + pin_b_->fall(callback(this,&rotaryinc::fallB)); +} + +void rotaryinc::riseA() +{ + pin_b_ -> read() ? pulse[1]-- : pulse[1]++; +} + +void rotaryinc::fallA() +{ + pin_b_ -> read() ? pulse[1]++ : pulse[1]--; +} + +void rotaryinc::riseB() +{ + pin_a_ -> read() ? pulse[1]++ : pulse[1]--; +} + +void rotaryinc::fallB() +{ + pin_a_ -> read() ? pulse[1]-- : pulse[1]++; +} + +double rotaryinc::getpulse() +{ + return pulse[1]/4; +} + +double rotaryinc::getspeed() +{ + speed = (M_PI * Diameter) * (pulse[1]/4 - pulse[0]) / Resolution / dt_; + pulse[0] = pulse[1]/4; + return speed; +} + +double rotaryinc::gettheta() +{ + theta[1] = pulse[1]/4 / Resolution * 360; + return theta[1]; +} + +double rotaryinc::getomega() +{ + omega = (theta[1] - theta[0]) / dt_; + theta[0] = theta[1]; + return omega; +} + +double rotaryinc::getrevolution() +{ + return pulse[1]/4 / Resolution; +} + +void rotaryinc::loop() +{ + time = timer.read(); + while(timer.read() - time <= dt_); +} + +rotaryinc::~rotaryinc(){ + pin_a_->disable_irq(); + pin_b_->disable_irq(); + delete pin_a_; + delete pin_b_; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rotaryinc.hpp Wed Aug 24 06:50:01 2022 +0000 @@ -0,0 +1,44 @@ +#ifndef ROTARY_INC_ +#define ROTARY_INC_ +#include <mbed.h> +#define M_PI 3.1415926535 + +#if 0 + ~example~ + rotaryinc name(ピン名1,ピン名2,オムニの直径,パルスの分解能,一回のプログラムの時間(Δt))でこのクラスを使うことができる + name.getpulse()でパルスの値を取得できる + name.getspeed()でモータの速さを取得できる + name.loop(一回のプログラムの時間(Δt))でΔt秒ごとのプログラムにできる +#endif + +class rotaryinc +{ +public: + rotaryinc(PinName pinA,PinName pinB,double,double,double); + ~rotaryinc(); + Timer timer; + double getpulse(); + double getspeed(); + double gettheta(); + double getomega(); + double getrevolution(); + void loop(); +private: + InterruptIn *pin_a_,*pin_b_; + double time; + double pulse[2]; + double speed; + void init(PinName,PinName); + void riseA(void); + void riseB(void); + void fallA(void); + void fallB(void); + double theta[2]; + double omega; + double Resolution; + double Diameter; + double dt_; + double revolution_num; +}; + +#endif \ No newline at end of file