DriveDistanceStraightCommandnot finishing

[dcowden]

Occasionally, DriveDistanceCommand will not finish, even though it appears to have moved. I think this is related to the issues with TurnRobotRelative not finishing

[dcowden]

looking more closely, I ssuspect that the command that didnt finish was not Drivedistance, but actually this one:

, new DriveYawToNearestPerpendicular(driveSubsystem, navxSubsystem)

[dcowden]

actually no, clearly DriveYawToNearestPerpendicular is finishing because isFinished has no logic and returns true immeidately. So it must have been the command after it ( DriveDistanceStraightCommand

[dcowden]

the theory here is that resetEncoders might not complete before the next command runs.
Suggested fix is to wait a bit before finishing the command that resets the encoders (?)

[mandrews281]

I agree, I think I've seen issues with the command finishing too soon during balancing. I think the resetEncoders isn't getting executed before the isFinished check is done, so previous distance can trigger the command completing. I suggest either a Timer or Counter to ensure we've completed >4-5 loops before checking the distances.

[dcowden]

ok for tonight, i'll code a solution to this. ideally we dont have to do it in every command, since this is a problem in lots of places.