For Rotaryencoder and Motor
Diff: Rotaryencoder.cpp
- Revision:
- 0:b1487fd792c9
diff -r 000000000000 -r b1487fd792c9 Rotaryencoder.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Rotaryencoder.cpp Sat Jul 16 06:38:44 2022 +0000 @@ -0,0 +1,53 @@ +#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; +} \ No newline at end of file