I am from FTC team 4251 Cougar Robotics and this is for the Instructables sponsorship program. Throughout our testing, we have found that the encoders we use on our FTC robot will occasionally output a random, erratic value. The result of this is that, while driving in autonomous, the robot will stop early in one of its moves. These are unpredictable incidents and only happen in one move in every 10-15 runs. This value is usually significantly higher than the surrounding values. The values would then go back to their usual or expected range after a couple of milliseconds. Now the problem with this is that whenever we would get one of these values, our autonomous driving would end early because the program thought that it had reached the target value, i.e. if the target value was 2000, we were climbing from 0, and the erratic value was 1,000,000. (include graph here and replace theoretical values with actual values) This would cause our autonomous to stop early and not complete a movement. Because we were able to identify this problem, we developed a very simple solution to solve it.

Step 1: Identify/View the Problem

To identify if your robot is suffering from these sporadic encoder values, there is a simple test (and we recommend implimenting the solution anyway so that if you ever do see it, it wont affect you). First, write a simple autonomous that just will spin the wheels forever and display encoder values.

// reset your encoders
nmotorEncoder[motorLeft] = 0;
nmotorEncoder[motorRight] = 0;
//pause to make sure the change has the opportunity to take affect
wait1msec(100);
// start your motors moving
motor[motorLeft] = 100;
motor[motorRight] = 100;
while(time100[T1] < 6000)
{
//send your encoder values to the debug stream
writeDebugStreamLine("%f %f",nmotorEncoder[motorLeft], nmotorEncoder[motorRight])
wait1msec(10);
}

Now to view the debug stream, go to your meuns Robot/Debug Windows/Debug Stream. If you cannot see that, you must go to Window/Menu Level/Super User first to get full access. Now, when you run this program with the NXT connected to your computer, the debug stream should fill up with values that you can then import into excel. once in excel or any other spreadsheet program, you can graph the results to easily see the outlier values. Don't expect to see the sporadic values every time. Remember, we found them once every 10-15 long autonomous runs.

Step 2: Fix the Problem: Theory

So if the values only appear for a few milliseconds, but still get caught and trigger our program to move on, how do we ignore the values we don't want? There is a simple solution. We can do one of a couple things. the simplest (the one we use) has had great success for our team. If our robot is driving to an encoder distance, and we see a value greater than our target, we wait a few milliseconds and check again. This gives the encoders time to get back to their normal values. If the current encoder value is less than the target we keep driving. However, if the encoder is still greater than the target, we know that we have actually got our target distance and move onto the next step in our autonomous. The first graph is what the encoder values should look like, with the motor running at a constant speed. The increase is linear. The second graph is shows the sporadic value as a spike down. The sporadic values can be up or down, in this case, it was down.

Step 3: Fix the Problem: Code

This is the movement function we use in autonomous. It takes an input of distance (calculated inches for ease of programming), speed, a target heading, and a brake/coast option. For this to work, an active heading is required, as well as a target heading.
The way this works combines the current encoder values to get a running total as the robot drives. The functions leftMotor() and rightMotor() get fed a number and set all of the drive motors to that speed. They could be replaced with "motor[motorLeft] = (speed + correction)" and "motor[motorRight] = (speed - correction)".

void moveDistanceAtSpeedOnHeading(float distance, int speed, float targetHeading, short goNoGo)
{
float diff;
float calculatedDistance;
float combinedEncoderDistance;
int correction;
calculatedDistance = abs(distance * 120);
diff = targetHeading - heading;
nMotorEncoder[motorLeft] = 0;
nMotorEncoder[motorRight] = 0;
wait1Msec(200);
for(;;)
    {
    combinedEncoderDistance = abs(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight]);
    if(combinedEncoderDistance > calculatedDistance)
      {
      while(combinedEncoderDistance == abs(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight]))
        {
        wait1Msec(5);
        }
      if(abs(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight]) > calculatedDistance)
        {
        //continueLoop = false;
        break;
        }
      }
    diff = targetHeading - heading;
    correction = diff * CORRECTIONGAIN;
    leftMotor(speed + correction);
    rightMotor(speed - correction);
    wait1Msec(5); //added a value of 5 because that is about twice as fast as the encoders update their values
    }
if(goNoGo == NO_GO)
    {
    leftMotor(0);
    rightMotor(0);
    }
}

Step 4: Conclusion

This Instuctable is aimed at FTC teams using encoders and having sporadic behavior from their robot. If you have specific questions please leave them in the comments below. If you use the code we have provided, please credit our team when talking about your autonomous. please feel free to modify and share the code as well.