For Rotaryencoder and Motor
Rotaryencoder.cpp@0:b1487fd792c9, 2022-07-16 (annotated)
- Committer:
- m2130
- Date:
- Sat Jul 16 06:38:44 2022 +0000
- Revision:
- 0:b1487fd792c9
For Rotaryencoder and Motor;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m2130 | 0:b1487fd792c9 | 1 | #include "Rotaryencoder.hpp" |
m2130 | 0:b1487fd792c9 | 2 | |
m2130 | 0:b1487fd792c9 | 3 | Rotary::Rotary(PinName pinA, PinName pinB, double diameter, double resolusion, double deltat) : |
m2130 | 0:b1487fd792c9 | 4 | cir(diameter * pi), res(4 * resolusion), dt(deltat){ |
m2130 | 0:b1487fd792c9 | 5 | pulse[0] = 0; pulse[1] = 0; |
m2130 | 0:b1487fd792c9 | 6 | theta[0] = 0; theta[1] = 0; |
m2130 | 0:b1487fd792c9 | 7 | spd = 0; |
m2130 | 0:b1487fd792c9 | 8 | anguspd = 0; |
m2130 | 0:b1487fd792c9 | 9 | pin_A = new InterruptIn(pinA, PullUp); |
m2130 | 0:b1487fd792c9 | 10 | pin_B = new InterruptIn(pinB, PullUp); |
m2130 | 0:b1487fd792c9 | 11 | pin_A -> rise(callback(this, &Rotary::riseA)); |
m2130 | 0:b1487fd792c9 | 12 | pin_A -> fall(callback(this, &Rotary::fallA)); |
m2130 | 0:b1487fd792c9 | 13 | pin_B -> rise(callback(this, &Rotary::riseB)); |
m2130 | 0:b1487fd792c9 | 14 | pin_B -> fall(callback(this, &Rotary::fallB)); |
m2130 | 0:b1487fd792c9 | 15 | } |
m2130 | 0:b1487fd792c9 | 16 | |
m2130 | 0:b1487fd792c9 | 17 | void Rotary::reset(){ |
m2130 | 0:b1487fd792c9 | 18 | pulse[1] = 0; |
m2130 | 0:b1487fd792c9 | 19 | } |
m2130 | 0:b1487fd792c9 | 20 | |
m2130 | 0:b1487fd792c9 | 21 | double Rotary::pulse_get(){return pulse[1]/4;} |
m2130 | 0:b1487fd792c9 | 22 | |
m2130 | 0:b1487fd792c9 | 23 | double Rotary::theta_get(){ |
m2130 | 0:b1487fd792c9 | 24 | theta[1] = 360 * pulse[1]/res; |
m2130 | 0:b1487fd792c9 | 25 | return theta[1]; |
m2130 | 0:b1487fd792c9 | 26 | } |
m2130 | 0:b1487fd792c9 | 27 | |
m2130 | 0:b1487fd792c9 | 28 | double Rotary::speed_get(){ |
m2130 | 0:b1487fd792c9 | 29 | spd = cir * ((pulse[1] - pulse[0])/res) / dt; |
m2130 | 0:b1487fd792c9 | 30 | pulse[0] = pulse[1]; |
m2130 | 0:b1487fd792c9 | 31 | return spd; |
m2130 | 0:b1487fd792c9 | 32 | } |
m2130 | 0:b1487fd792c9 | 33 | |
m2130 | 0:b1487fd792c9 | 34 | double Rotary::angularspd_get(){ |
m2130 | 0:b1487fd792c9 | 35 | anguspd = (theta[1] - theta[0])/dt; |
m2130 | 0:b1487fd792c9 | 36 | theta[0] = theta[1]; |
m2130 | 0:b1487fd792c9 | 37 | return anguspd; |
m2130 | 0:b1487fd792c9 | 38 | } |
m2130 | 0:b1487fd792c9 | 39 | |
m2130 | 0:b1487fd792c9 | 40 | void Rotary::riseA(){pin_B -> read() ? pulse[1]-- : pulse[1]++;} |
m2130 | 0:b1487fd792c9 | 41 | |
m2130 | 0:b1487fd792c9 | 42 | void Rotary::fallA(){pin_B -> read() ? pulse[1]++ : pulse[1]--;} |
m2130 | 0:b1487fd792c9 | 43 | |
m2130 | 0:b1487fd792c9 | 44 | void Rotary::riseB(){pin_A -> read() ? pulse[1]++ : pulse[1]--;} |
m2130 | 0:b1487fd792c9 | 45 | |
m2130 | 0:b1487fd792c9 | 46 | void Rotary::fallB(){pin_A -> read() ? pulse[1]-- : pulse[1]++;} |
m2130 | 0:b1487fd792c9 | 47 | |
m2130 | 0:b1487fd792c9 | 48 | Rotary::~Rotary(){ |
m2130 | 0:b1487fd792c9 | 49 | pin_A -> disable_irq(); |
m2130 | 0:b1487fd792c9 | 50 | pin_B -> disable_irq(); |
m2130 | 0:b1487fd792c9 | 51 | delete pin_A; |
m2130 | 0:b1487fd792c9 | 52 | delete pin_B; |
m2130 | 0:b1487fd792c9 | 53 | } |