Skip to content

Instantly share code, notes, and snippets.

@TheAlchenmist
Last active August 29, 2015 14:15
Show Gist options
  • Save TheAlchenmist/5a89d2cf0bbf2e36a165 to your computer and use it in GitHub Desktop.
Save TheAlchenmist/5a89d2cf0bbf2e36a165 to your computer and use it in GitHub Desktop.
#define ENC_ERROR_THRESHOLD 3000
int checkEnc = nMotorEncoder[curMotor];
int knownGoodEnc = motorStates[curMotor].lastRealEncoderPos;
int curEnc;
if (abs(checkEnc - knownGoodEnc) > ENC_ERROR_THRESHOLD) {
//do nothing because enc val is bad
} else {
curEnc = checkEnc; //enc val is good
motorStates[curMotor].encoder += (curEnc - motorStates[curMotor].lastRealEncoderPos);
motorStates[curMotor].lastRealEncoderPos = curEnc;
if (abs(curEnc) > 30000) { //reset real encoder when it overflows
motorStates[curMotor].lastRealEncoderPos = 0;
nMotorEncoder[curMotor] = 0;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment