Fix Encoder Driver

This commit is contained in:
Aadi Desai 2022-03-25 14:55:43 +00:00
parent 4e9e486c96
commit 4d0cc666f6
No known key found for this signature in database
GPG key ID: CFFFE425830EF4D9
2 changed files with 64 additions and 32 deletions

View file

@ -3,13 +3,18 @@
class Knob { class Knob {
private: private:
int rotation; int rotation, rotationInternal;
int minimum, maximum; int minimum, maximum, previousRotation;
bool A, B; bool A, B;
bool rotPlusOnePrev, rotMinOnePrev; enum PrevRot {
NONE,
CW,
ACW,
};
public: public:
Knob(int minimum, int max); Knob(int minimum, int maximum, int initialRotation);
Knob(int minimum, int maximum) : Knob(minimum, maximum, minimum) {} // Delegate to full constructor, using minimum as initial rotation
int getRotation(); int getRotation();

View file

@ -1,13 +1,13 @@
#include <knob> #include <knob>
Knob::Knob(int minimum, int maximum) { Knob::Knob(int minimum, int maximum, int initialRotation) {
Knob::minimum = minimum; Knob::minimum = minimum;
Knob::maximum = maximum; Knob::maximum = maximum;
Knob::A = false; Knob::A = false;
Knob::B = false; Knob::B = false;
Knob::rotPlusOnePrev = false; Knob::previousRotation = NONE;
Knob::rotMinOnePrev = false; Knob::rotation = initialRotation;
Knob::rotation = 0; Knob::rotationInternal = initialRotation;
} }
int Knob::getRotation() { int Knob::getRotation() {
@ -15,34 +15,61 @@ int Knob::getRotation() {
}; };
void Knob::updateRotation(bool ANew, bool BNew) { void Knob::updateRotation(bool ANew, bool BNew) {
bool rotPlusOneNew = (!B && !A && !BNew && ANew) || if (A == ANew && B == BNew)
return; // No change, do not update values
bool fullstep = (BNew && ANew) || (!BNew && !ANew);
bool cwRot = (!B && !A && !BNew && ANew) ||
(!B && A && BNew && ANew) || (!B && A && BNew && ANew) ||
(B && !A && !BNew && !ANew) || (B && !A && !BNew && !ANew) ||
(B && A && BNew && !ANew); (B && A && BNew && !ANew);
bool rotMinOneNew = (!B && !A && BNew && !ANew) || bool acwRot = (!B && !A && BNew && !ANew) ||
(!B && A && !BNew && !ANew) || (!B && A && !BNew && !ANew) ||
(B && !A && BNew && ANew) || (B && !A && BNew && ANew) ||
(B && A && !BNew && ANew); (B && A && !BNew && ANew);
bool impossibleState = (!B && !A && BNew && ANew) || bool impossible = (!B && !A && BNew && ANew) ||
(!B && A && BNew && !ANew) || (!B && A && BNew && !ANew) ||
(B && !A && !BNew && ANew) || (B && !A && !BNew && ANew) ||
(B && A && !BNew && !ANew); (B && A && !BNew && !ANew);
if (rotPlusOneNew || (impossibleState && rotPlusOnePrev)) if (cwRot) {
rotation += 2; if (previousRotation == CW && fullstep) {
if (rotMinOneNew || (impossibleState && rotMinOnePrev)) rotationInternal++;
rotation -= 2; } else if (previousRotation == ACW) {
if (rotation < minimum) previousRotation = NONE;
rotation = minimum; } else {
if (rotation > maximum) previousRotation = CW;
rotation = maximum; }
} else if (acwRot) {
if (previousRotation == ACW && fullstep) {
rotationInternal--;
} else if (previousRotation == CW) {
previousRotation = NONE;
} else {
previousRotation = ACW;
}
} else if (impossible) {
if (fullstep) {
switch (previousRotation) {
case NONE: // Step skipped and no previous direction
break;
case CW:
rotationInternal++;
break;
case ACW:
rotationInternal--;
break;
}
}
}
A = ANew; A = ANew;
B = BNew; B = BNew;
if (!impossibleState) { if (rotationInternal < minimum)
rotPlusOnePrev = rotPlusOneNew; rotationInternal = minimum;
rotMinOnePrev = rotMinOneNew; if (rotationInternal > maximum)
} rotationInternal = maximum;
rotation = rotationInternal;
} }