Package edu.wpi.first.wpilibj2.command

Class MecanumControllerCommand

java.lang.Object
edu.wpi.first.wpilibj2.command.CommandBase
edu.wpi.first.wpilibj2.command.MecanumControllerCommand
All Implemented Interfaces:
Sendable, Command
public class MecanumControllerCommand
extends CommandBase
A command that uses two PID controllers (PIDController) and a ProfiledPIDController (ProfiledPIDController) to follow a trajectory Trajectory with a mecanum drive.

The command handles trajectory-following, Velocity PID calculations, and feedforwards internally. This is intended to be a more-or-less "complete solution" that can be used by teams without a great deal of controls expertise.

Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID functionality of a "smart" motor controller) may use the secondary constructor that omits the PID and feedforward functionality, returning only the raw wheel speeds from the PID controllers.

The robot angle controller does not follow the angle given by the trajectory but rather goes to the angle given in the final state of the trajectory.

  • Constructor Details

    • MecanumControllerCommand

      public MecanumControllerCommand​(Trajectory trajectory, Supplier<Pose2d> pose, SimpleMotorFeedforward feedforward, MecanumDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, Supplier<Rotation2d> desiredRotation, double maxWheelVelocityMetersPerSecond, PIDController frontLeftController, PIDController rearLeftController, PIDController frontRightController, PIDController rearRightController, Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds, Consumer<MecanumDriveMotorVoltages> outputDriveVoltages, Subsystem... requirements)
      Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to 12 as a voltage output to the motor.

      Note: The controllers will *not* set the outputVolts to zero upon completion of the path this is left to the user, since it is not appropriate for paths with nonstationary endstates.

      Parameters:
      trajectory - The trajectory to follow.
      pose - A function that supplies the robot pose - use one of the odometry classes to provide this.
      feedforward - The feedforward to use for the drivetrain.
      kinematics - The kinematics for the robot drivetrain.
      xController - The Trajectory Tracker PID controller for the robot's x position.
      yController - The Trajectory Tracker PID controller for the robot's y position.
      thetaController - The Trajectory Tracker PID controller for angle for the robot.
      desiredRotation - The angle that the robot should be facing. This is sampled at each time step.
      maxWheelVelocityMetersPerSecond - The maximum velocity of a drivetrain wheel.
      frontLeftController - The front left wheel velocity PID.
      rearLeftController - The rear left wheel velocity PID.
      frontRightController - The front right wheel velocity PID.
      rearRightController - The rear right wheel velocity PID.
      currentWheelSpeeds - A MecanumDriveWheelSpeeds object containing the current wheel speeds.
      outputDriveVoltages - A MecanumDriveMotorVoltages object containing the output motor voltages.
      requirements - The subsystems to require.

    • MecanumControllerCommand

      public MecanumControllerCommand​(Trajectory trajectory, Supplier<Pose2d> pose, SimpleMotorFeedforward feedforward, MecanumDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, double maxWheelVelocityMetersPerSecond, PIDController frontLeftController, PIDController rearLeftController, PIDController frontRightController, PIDController rearRightController, Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds, Consumer<MecanumDriveMotorVoltages> outputDriveVoltages, Subsystem... requirements)
      Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. PID control and feedforward are handled internally. Outputs are scaled from -12 to 12 as a voltage output to the motor.

      Note: The controllers will *not* set the outputVolts to zero upon completion of the path this is left to the user, since it is not appropriate for paths with nonstationary endstates.

      Note 2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.

      Parameters:
      trajectory - The trajectory to follow.
      pose - A function that supplies the robot pose - use one of the odometry classes to provide this.
      feedforward - The feedforward to use for the drivetrain.
      kinematics - The kinematics for the robot drivetrain.
      xController - The Trajectory Tracker PID controller for the robot's x position.
      yController - The Trajectory Tracker PID controller for the robot's y position.
      thetaController - The Trajectory Tracker PID controller for angle for the robot.
      maxWheelVelocityMetersPerSecond - The maximum velocity of a drivetrain wheel.
      frontLeftController - The front left wheel velocity PID.
      rearLeftController - The rear left wheel velocity PID.
      frontRightController - The front right wheel velocity PID.
      rearRightController - The rear right wheel velocity PID.
      currentWheelSpeeds - A MecanumDriveWheelSpeeds object containing the current wheel speeds.
      outputDriveVoltages - A MecanumDriveMotorVoltages object containing the output motor voltages.
      requirements - The subsystems to require.

    • MecanumControllerCommand

      public MecanumControllerCommand​(Trajectory trajectory, Supplier<Pose2d> pose, MecanumDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, Supplier<Rotation2d> desiredRotation, double maxWheelVelocityMetersPerSecond, Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds, Subsystem... requirements)
      Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. The user should implement a velocity PID on the desired output wheel velocities.

      Note: The controllers will *not* set the outputVolts to zero upon completion of the path - this is left to the user, since it is not appropriate for paths with non-stationary end-states.

      Parameters:
      trajectory - The trajectory to follow.
      pose - A function that supplies the robot pose - use one of the odometry classes to provide this.
      kinematics - The kinematics for the robot drivetrain.
      xController - The Trajectory Tracker PID controller for the robot's x position.
      yController - The Trajectory Tracker PID controller for the robot's y position.
      thetaController - The Trajectory Tracker PID controller for angle for the robot.
      desiredRotation - The angle that the robot should be facing. This is sampled at each time step.
      maxWheelVelocityMetersPerSecond - The maximum velocity of a drivetrain wheel.
      outputWheelSpeeds - A MecanumDriveWheelSpeeds object containing the output wheel speeds.
      requirements - The subsystems to require.

    • MecanumControllerCommand

      public MecanumControllerCommand​(Trajectory trajectory, Supplier<Pose2d> pose, MecanumDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, double maxWheelVelocityMetersPerSecond, Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds, Subsystem... requirements)
      Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory. The user should implement a velocity PID on the desired output wheel velocities.

      Note: The controllers will *not* set the outputVolts to zero upon completion of the path - this is left to the user, since it is not appropriate for paths with non-stationary end-states.

      Note 2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.

      Parameters:
      trajectory - The trajectory to follow.
      pose - A function that supplies the robot pose - use one of the odometry classes to provide this.
      kinematics - The kinematics for the robot drivetrain.
      xController - The Trajectory Tracker PID controller for the robot's x position.
      yController - The Trajectory Tracker PID controller for the robot's y position.
      thetaController - The Trajectory Tracker PID controller for angle for the robot.
      maxWheelVelocityMetersPerSecond - The maximum velocity of a drivetrain wheel.
      outputWheelSpeeds - A MecanumDriveWheelSpeeds object containing the output wheel speeds.
      requirements - The subsystems to require.

  • Method Details

    • initialize

      public void initialize()
      Description copied from interface: Command
      The initial subroutine of a command. Called once when the command is initially scheduled.

    • execute

      public void execute()
      Description copied from interface: Command
      The main body of a command. Called repeatedly while the command is scheduled.

    • end

      public void end​(boolean interrupted)
      Description copied from interface: Command
      The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled.

      Do not schedule commands here that share requirements with this command. Use Command.andThen(Command...) instead.

      Parameters:
      interrupted - whether the command was interrupted/canceled

    • isFinished

      public boolean isFinished()
      Description copied from interface: Command
      Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.
      Returns:
      whether the command has finished.