[mschwab013]

Originally Posted by Chris Hibner

You can use the encoders to emulate a gyro for driving straight. Here is some psuedo-code for how to do it:

heading = 57.3*(leftEncDist - rightEncDist) / trackWidth;

if (abs(driverTurnCmd) < turnDeadZone)
{
  if (headingCaptured == FALSE)
  {
     desiredHeading = heading;
     headingCaptured = TRUE;
  }
  turnCmdPID = PID(heading, desiredHeading);
  arcadeDrive(driverThrottleCmd, turnCmdPID);
}
else
{
  headingCaptured = FALSE;
  arcadeDrive(driverThrottleCmd, driverTurnCmd);
}

When we've done this in the past, it makes it a little smoother if you lead your desired heading with your turn rate. So instead of "desiredHeading = heading" in the above code, we've done "desiredHeading = Heading + robotTurnSpeed * leadFactor", where "leadFactor" is a calibration that you tune to make the stopping of the turn smooth.

I'm currently trying to implement a very similar system and was wondering if you had any recommendations for tuning the PID constants and the "leadFactor" at the same time