For Rotaryencoder and Motor
Rotaryencoder.cpp
- Committer:
- m2130
- Date:
- 2022-07-16
- Revision:
- 0:b1487fd792c9
File content as of revision 0:b1487fd792c9:
#include "Rotaryencoder.hpp" Rotary::Rotary(PinName pinA, PinName pinB, double diameter, double resolusion, double deltat) : cir(diameter * pi), res(4 * resolusion), dt(deltat){ pulse[0] = 0; pulse[1] = 0; theta[0] = 0; theta[1] = 0; spd = 0; anguspd = 0; pin_A = new InterruptIn(pinA, PullUp); pin_B = new InterruptIn(pinB, PullUp); pin_A -> rise(callback(this, &Rotary::riseA)); pin_A -> fall(callback(this, &Rotary::fallA)); pin_B -> rise(callback(this, &Rotary::riseB)); pin_B -> fall(callback(this, &Rotary::fallB)); } void Rotary::reset(){ pulse[1] = 0; } double Rotary::pulse_get(){return pulse[1]/4;} double Rotary::theta_get(){ theta[1] = 360 * pulse[1]/res; return theta[1]; } double Rotary::speed_get(){ spd = cir * ((pulse[1] - pulse[0])/res) / dt; pulse[0] = pulse[1]; return spd; } double Rotary::angularspd_get(){ anguspd = (theta[1] - theta[0])/dt; theta[0] = theta[1]; return anguspd; } void Rotary::riseA(){pin_B -> read() ? pulse[1]-- : pulse[1]++;} void Rotary::fallA(){pin_B -> read() ? pulse[1]++ : pulse[1]--;} void Rotary::riseB(){pin_A -> read() ? pulse[1]++ : pulse[1]--;} void Rotary::fallB(){pin_A -> read() ? pulse[1]-- : pulse[1]++;} Rotary::~Rotary(){ pin_A -> disable_irq(); pin_B -> disable_irq(); delete pin_A; delete pin_B; }