EncoderCheck.c

#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;
	}
}