WPILib Trajectory Generation Question

[ahaltom]

Hello, we are experiencing some strange behavior while running our Swerve MK4i autonomous and are wondering if anyone could provide some suggestions.

For example, here is the same auton run on two separate occasions. We have the autonomous mode split between three separate paths, but the distance for the third path appears to be quite different each time we run it. We suspect at this point that it may be an encoder issue, but we aren’t too sure due to the inconsistency.

Working auton

Robot crash

I will add below what I believe are the relevant pieces of code and also our Github repo (beware it’s messy).

vscodeprojects/MK4I Drive Only/src/main/java/frc/robot at master ·...

Autonomous command
public Command getAutonomousCommand() {

m_shooter.zeroEncoderOfHood();
m_drivetrainSubsystem.zeroGyroscope();
// 1. Create trajectory settings
TrajectoryConfig trajectoryConfig = Constants.auto.follower.T_CONFIG;

// 2. Generate trajectory
Trajectory trajectory1 = TrajectoryGenerator.generateTrajectory(
  new Pose2d(0, 0, new Rotation2d(0)),
  List.of(
      new Translation2d(-.5, 0),
      new Translation2d(-.75, 0)),
  new Pose2d(-1, 0, Rotation2d.fromDegrees(-10)),
  trajectoryConfig);

  Trajectory trajectory2 = TrajectoryGenerator.generateTrajectory(
  new Pose2d(-1, 0, new Rotation2d(-10)),
  List.of(
      new Translation2d(-.3, .75),
      new Translation2d(.2, 1.5)),
  new Pose2d(0.35, 1.75, Rotation2d.fromDegrees(-70)), //.4 2.15
  trajectoryConfig);

  Trajectory trajectory3 = TrajectoryGenerator.generateTrajectory(
  new Pose2d(0, 0, new Rotation2d(-70)), //.35 1.75
  List.of(
      new Translation2d(-.9 * .3, 2.5 * .3),
      new Translation2d(-.9 * .6, 2.5 * .6)),
      //Closer to tower decrease 5.5
      //Closer to middle of field increase -.5
  //5.6 worked then ran into wall I suggest lowering it by 1 and tuning from there
  new Pose2d(-0.9, 2.5, Rotation2d.fromDegrees(-70)),
  trajectoryConfig);


// 3. Define PID controllers for tracking trajectory
PIDController xController = Constants.auto.follower.X_PID_CONTROLLER;
PIDController yController = Constants.auto.follower.Y_PID_CONTROLLER;
ProfiledPIDController thetaController = Constants.auto.follower.ROT_PID_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);

// 4. Construct command to follow trajectory
SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(
  trajectory1,
  m_drivetrainSubsystem::getPose2d,
  m_drivetrainSubsystem.getKinematics(),
  xController,
  yController,
  thetaController,
  m_drivetrainSubsystem::setAllStates,
  m_drivetrainSubsystem);

SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand(
  trajectory2,
  m_drivetrainSubsystem::getPose2d,
  m_drivetrainSubsystem.getKinematics(),
  xController,
  yController,
  thetaController,
  m_drivetrainSubsystem::setAllStates,
  m_drivetrainSubsystem);

  SwerveControllerCommand swerveControllerCommand3 = new SwerveControllerCommand(
trajectory3,
m_drivetrainSubsystem::getPose2d,
m_drivetrainSubsystem.getKinematics(),
xController,
yController,
thetaController,
m_drivetrainSubsystem::setAllStates,
m_drivetrainSubsystem); 

  m_shooter.moveHood(0);
  m_shooter.stop();
  m_magazine.runUpperMag(0);
  return new SequentialCommandGroup(
new ParallelRaceGroup(
  new AutoPickUpBall(m_intake, m_magazine, m_shooter,11500,-4),
  new SequentialCommandGroup(
    new InstantCommand(() -> m_drivetrainSubsystem.resetOdometry(trajectory1.getInitialPose())),
    swerveControllerCommand1,
    new InstantCommand(() -> m_drivetrainSubsystem.stopModules()),
    new AutoShootCommand(m_magazine, m_shooter, 11500),
    new WaitCommand(1),
    new AutoShootCommand(m_magazine, m_shooter, 11500),
    new InstantCommand(() -> m_drivetrainSubsystem.resetOdometry(trajectory2.getInitialPose())),
    swerveControllerCommand2,
    new InstantCommand(() -> m_drivetrainSubsystem.stopModules()),
    new WaitCommand(1),
    new AutoShootCommand(m_magazine, m_shooter, 11500)
  )
),
new ParallelCommandGroup(
  new AutoPickUpBall(m_intake, m_magazine, m_shooter,16000,-8.2),
  new SequentialCommandGroup(
    new InstantCommand(() -> m_drivetrainSubsystem.resetOdometry(trajectory3.getInitialPose())),
    swerveControllerCommand3, 
    new InstantCommand(() -> m_drivetrainSubsystem.stopModules()),
    new WaitCommand(1),
    new AutoShootCommand(m_magazine, m_shooter, 16000),
    new WaitCommand(1),
    new AutoShootCommand(m_magazine, m_shooter, 16000) 
  )
)
  );


  // return new AutoPickUpBall(m_intake, m_magazine);

// 5. Add some init and wrap-up, and return everything
}

[jdaming]

Can you test your repeatability on just like a 10ft (you pick a number just a decent distance) straight path? Did you get a chance to tune your X and Y Controllers? Those should be the ones handling that.