Encodercheck 2-15

encodercheck2-15.c

//Check for sporadic encoder values as documented by Cougar Robotics #623
int checkEnc = nMotorEncoder[curMotor];
int curEnc = nMotorEncoder[curMotor];

if (pow > 0) { //we expect encoder value to be changing
  while (true) {
    //This can potentially take a long time, so set time limit
    if ((nPgmTime - encFnStartTimeMs) > 5) { 
      break;
    }
    while (curEnc == checkEnc) { //loop until we see a different encoder value
      //This can also potentially take a long time, so set time limit
      if ((nPgmTime - encFnStartTimeMs) > 5) { 
        break;
      }
      curEnc = nMotorEncoder[curMotor];
    }
    //check to see if the different encoder value is changing correctly
    if ((sgn(curEnc - checkEnc) == sgn(motor[curMotor])) { 
      break;
    } else { //changed incorrectly, so repeat check on new encoder
      checkEnc = curEnc; 
    }
  }
} else { //we expect encoder value to be still
  //monitor for short period
  while ((nPgmTime - encFnStartTimeMs) < 5) {
    curEnc = nMotorEncoder[curMotor];
    if (curEnc != checkEnc) {
      checkEnc = curEnc; //assume latest value is good
    }
  }
}