Robot stuttering when following a straight path

[Joel_Singh]

Hello! My team is trying to get our swerve bot to follow a simple path to the point 1 meter in front of the robot. We’re using the YAGSL library. We have an issue where the wheels of the robot stutter for about half a meter and then just stop.

Here’s a video showcasing the issue:

robot stuttering when following.mp4

Here is the command we’re using to generate the path, taken from the YAGSL example code:

  public Command driveToPose(Pose2d pose)
  {
// Create the constraints to use while pathfinding
    PathConstraints constraints = new PathConstraints(
        swerveDrive.getMaximumVelocity(), 4.0,
        swerveDrive.getMaximumAngularVelocity(), Units.degreesToRadians(720));

// Since AutoBuilder is configured, we can use it to build pathfinding commands
    return AutoBuilder.pathfindToPose(
        pose,
        constraints,
        0.0, // Goal end velocity in meters/sec
        0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
                                     );
  }

And here is how we’re calling it:

drivetrain.driveToPose(
      new Pose2d(
        new Translation2d(1, 0), new Rotation2d()
      )
    )

Any ideas of what could be causing the stuttering? I will also note that our teleop code works and that the robot pose (being viewed through shuffleboard) looks correct.

Thank you in advance!

[nstrike]

Huh, have you tried disabling cosine compensation just for this. The problem will be easier to debug in FRC Web Components or Elastic too

[Joel_Singh]

Thanks for the response, I’ll try disabling cosine compensation. Also, could you tell me more about how we could go about debugging this in FRC Web Components? Like what’s available for me to use to debug or how you would go about figuring out the issue. I know the basics of putting values on shuffleboard but not much else.

[nstrike]

@Joel_Singh

I know the basics of putting values on shuffleboard but not much else.

Same concept except JSON files. You can do it in code if you want but its more work than it’s worth.

FRC Web Components

[Ashray.g]

Something I’ve seen that can cause this exact behavior is using a blocking method (usually a configuration call to a device on CAN) inside the periodic methods.

[nstrike]

YAGSL does bloxk but it’s threaded and the pose estimator is accessed with mutex’s

[Joel_Singh]

oooh, thanks for the link, frc web components looks very useful. I’ll use this to aid in debugging