miki sumito / Mbed 2 deprecated four_wheel_omni

四輪オムニの足回りを動かすプログラム

Dependencies:   mbed

rotaryinc/rotaryinc.cpp@0:39c2bb18192b, 2022-10-08 (annotated)

Committer:
m_smt
Date:
Sat Oct 08 11:13:31 2022 +0000
Revision:
0:39c2bb18192b
move four wheel omni

Who changed what in which revision?

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