WaitCommand moves motors when it should be waiting

[alex5572]

We have a swerve bot using mk4 modules and running the WPILib SwerveControllerCommand. During autonomous which uses a SequentialCommandGroup, we run a Trajectory then a WaitCommand. However, when the Trajectory is over and the WaitCommand starts, the robot drifts in the last direction it was going when the Trajectory finished. Is this a problem with the WaitCommand or something else?

    public class ultrasonicAuto extends SequentialCommandGroup {
    public ultrasonicAuto(Swerve s_Swerve, Ultrasonic ultrasonic, Timer timer){
        System.out.println("Ultrasonic Auto !!");
        TrajectoryConfig config =
            new TrajectoryConfig(
                    Constants.AutoConstants.kMaxSpeedMetersPerSecond,
                    Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
                .setKinematics(Constants.Swerve.swerveKinematics);

        Trajectory firstHalfTrajectory =
            TrajectoryGenerator.generateTrajectory(

                List.of(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(1, 0, new Rotation2d(0)), new Pose2d(1, 1, new Rotation2d(90))),
                config);

        var thetaController = new ProfiledPIDController(Constants.AutoConstants.kPThetaController, 0, 0, Constants.AutoConstants.kThetaControllerConstraints);
        thetaController.enableContinuousInput(-Math.PI, Math.PI);
        SwerveControllerCommand firstHalfTraject = new SwerveControllerCommand(firstHalfTrajectory, s_Swerve::getPose, Constants.Swerve.swerveKinematics, new PIDController(Constants.AutoConstants.kPXController, 0, 0), new PIDController(Constants.AutoConstants.kPYController, 0, 0), thetaController, s_Swerve::setModuleStates, s_Swerve);
        
        WaitCommand firstWait = new WaitCommand(3);
        WaitCommand secondWait = new WaitCommand(3);

        addCommands(
            new InstantCommand(() -> s_Swerve.resetOdometry(firstHalfTrajectory.getInitialPose())),
            firstWait,
            firstHalfTraject ,
            secondWait
        );
    }
}

[Fletch1373]

I suspect the “drift” you describe is the inertia of the bot carrying it in that direction (likely combined with motors in coast mode)

Do you zero the motor outputs to stop the motors after the trajectory? (I don’t see it, but may have missed something) If not, I recommend doing so.

[alex5572]

The motors are all set on brake mode. It is not drifting but instead keeps moving the robot in the direction of which the Trajectory ended during the duration of the WaitCommand. The motors keep getting power but don’t move as fast as during the Trajectory, instead they stutter and move slowly, but all in sync and stop when the WaitCommand is over.

[Oblarg]

Your motors are continuing on the last output they were sent before the WaitCommand starts. Put a command between the two that sets the outputs to zero.

[Fletch1373]

Yeah, if they remain actively powered, however slight that may be, then it’s retaining the last command given to it by the trajectory (as alluded to in my last comment and called out explicitly by @Oblarg)

[Bmongar]

I think @Oblarg is right. Send a command with a Chassis speed set to all 0 to your drive method.

[veg]

I have usually seen the motor speeds set to zero in the end() method of the Command. Then you don’t need to insert a separate command in the sequence.