WPI Swerve Controller

Our team has decided to use swerve drive this year and for the most part it has been great. The problem is trajectory following. When we give the robot a path for driving 1 meter forward or something like that it does fine, but if we give it a more complex set of waypoints it does random things. Sometimes it drives full power in the wrong direction, sometimes it follows some path but not the one we gave it. The odometry is always accurate, during and after the command it shows the correct position. We can PID to points on the field using our own command and the robot does this pretty well, the problem is just doing that for a trajectory. After trying lots of things (SwerveControllerCommand and HolonomicController with cubic and quintic spline trajectories each) we just copied and pasted the WPI example which also didn’t work. Below is the code we used (WPI example but with altered points).

EDIT: I forgot to add, it does different things when I redeploy the code even if I don’t change the code, but if I don’t redeploy and just run the auto over again it works, or it doesn’t work but in the same way each time.

/**

  • Use this to pass the autonomous command to the main {@link Robot} class.
  • @return the command to run in autonomous
    */

public Command getAutonomousCommand() {

// An example trajectory to follow.  All units in meters.
Trajectory exampleTrajectory =
    TrajectoryGenerator.generateTrajectory(
        new Pose2d(0.0, 0.0, new Rotation2d(-Math.PI / 2.0)),
        List.of(
          new Translation2d(0.0, -0.5),
          new Translation2d(1.0, 0.5)
        ),
        new Pose2d(1.0, 0.0, new Rotation2d(Math.PI / 2.0)),
        Constants.drive.auto.CONFIG);

var thetaController =
    new ProfiledPIDController(
        0.2, 0, 0, new Constraints(Constants.drive.MAX_RADIANS, Constants.drive.MAX_RADIANS * 2.0));
thetaController.enableContinuousInput(-Math.PI, Math.PI);

SwerveControllerCommand swerveControllerCommand =
    new SwerveControllerCommand(
        exampleTrajectory,
        drivetrain::getPose, // Functional interface to feed supplier
        drivetrain.getKinematics(),
        // Position controllers 
        new PIDController(4.0, 0.0, 0.0),
        new PIDController(4.0, 0.0, 0.0),
        thetaController,
        drivetrain::setModuleStates,
        drivetrain);

// Reset odometry to the starting pose of the trajectory.
drivetrain.setPose(exampleTrajectory.getInitialPose());

// Run path following command, then stop at the end.
return swerveControllerCommand.andThen(() -> drivetrain.drive(0, 0, 0, false));

}