Issue with WPILib trajectory following - Robot drives too short

Our team is having some trouble with our trajectory following. We’re using the RamseteCommand, with characterization values from SysId. The issue is, whenever we tell our robot to follow a straight path, it always ends up a little short of that path. It seems that the robot travels about 20% less than the distance it is supposed to travel. We’ve concluded that our encoders and gearings are not an issue, because simply pushing the robot 2 meters causes the encoders to read the correct values. We’ve also tried characterizing 3 times, and every time the values are similar (+/- 0.01).

This is what our SysId window looks like

Our RamseteTrajectoryCommand

public class RamseteTrajectoryCommand extends RamseteCommand {
  private Trajectory trajectory;
  private Drivetrain drivetrain;

  public RamseteTrajectoryCommand(Drivetrain drivetrain, Trajectory trajectory) {
    super(
      trajectory,
      drivetrain::getCurrentPose,
      new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
      new SimpleMotorFeedforward(DriveConstants.ksVolts, DriveConstants.kvVoltSecondsPerMeter, DriveConstants.kaVoltSecondsSquaredPerMeter),
      DriveConstants.kDriveKinematics,
      drivetrain::getWheelSpeeds,
      new PIDController(DriveConstants.kPDriveVel, 0, 0),
      new PIDController(DriveConstants.kPDriveVel, 0, 0),
      drivetrain::tankDriveVolts,
      drivetrain
    );
    this.drivetrain = drivetrain;
    this.trajectory = trajectory;
  }

  @Override
  public void initialize() {
    super.initialize();
    drivetrain.resetEncoders();
    drivetrain.resetOdometry(trajectory.getInitialPose());
  }
}

Any ideas? We’ve been working at this for a while and can’t seem to make any progress.

Any help would be greatly appreciated!

Does your odometry reading report that you are short after the trajectory finishes? If so, you probably need to increase P.
If odometry does not report that you are short, and seems to make it to the final pose, you probably have some sort of problem with gearing that is being missed. If your in-code wheel diameters are larger than reality, for example, you might be driving short and your controller just doesn’t know it.
Those are problems we’ve seen.

In doing a code review this morning, I found we used 10:1 gearing in the tooling but in our drivetrain code we had an old gear ratio of 7.8. Changing the gear ratio in the code should fix the issue I suspect.

2 Likes

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.