diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java b/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java deleted file mode 100644 index eaa2735..0000000 --- a/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java +++ /dev/null @@ -1,43 +0,0 @@ -// package frc.robot.drivetrain.commands; - -// import static com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType.MotionMagicExpo; -// import static com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.Velocity; -// import static frc.robot.drivetrain.TunerConstants.DriveCommandsConstants; - -// import java.util.function.Supplier; - -// import com.ctre.phoenix6.swerve.SwerveRequest; -// import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; - -// import edu.wpi.first.math.controller.ProfiledPIDController; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.drivetrain.CommandSwerveDrivetrain; - -// public class DriveToPose extends Command{ - -// private final ProfiledPIDController xController; -// private final ProfiledPIDController yController; -// private final ProfiledPIDController omegaController; -// private final ForwardPerspectiveValue perspectiveValue; - -// private final CommandSwerveDrivetrain swerveDrivetrain; -// private final SwerveRequest.FieldCentric fieldCentricSwerveRequest; - -// private final Supplier poseSupplier; -// // = new SwerveRequest.FieldCentric() -// // .withSteerRequestType(MotionMagicExpo) -// // .withDriveRequestType(Velocity) -// // .withForwardPerspective(perspectiveValue); - - -// /** -// * Constructs a DriveToPoseCommand -// * -// * @param drivetrainSubsystem drivetrain subsystem -// * @param goalPose goal pose to drive to -// */ -// public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider) { -// this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); -// } -// } diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java new file mode 100644 index 0000000..f61bdef --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/commands/DriveToPoseCommand.java @@ -0,0 +1,145 @@ +package frc.robot.drivetrain.commands; + + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static frc.robot.vision.VisionConfig.AlignmentConfig.*; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.drivetrain.CommandSwerveDrivetrain; + +import java.util.function.Supplier; + +/** + * Command to drive to a pose. + */ +public class DriveToPoseCommand extends Command { + + private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); + private static final Angle THETA_TOLERANCE = Degrees.of(1.0); + + protected static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), + MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); + protected static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), + MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); + + private final ProfiledPIDController xController; + private final ProfiledPIDController yController; + private final ProfiledPIDController thetaController; + + private final CommandSwerveDrivetrain drivetrainSubsystem; + private final FieldCentric fieldCentricSwerveRequest = new FieldCentric() + .withSteerRequestType(SteerRequestType.MotionMagicExpo) + .withDriveRequestType(DriveRequestType.Velocity) + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); // Always Blue coordinate system for auto drive + protected final Supplier poseProvider; + + /** + * Constructs a DriveToPoseCommand + * + * @param drivetrainSubsystem drivetrain subsystem + * @param goalPose goal pose to drive to + */ + public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider) { + this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); + } + + /** + * Constructs a DriveToPoseCommand with specific motion profile constraints + * + * @param drivetrainSubsystem drivetrain subsystem + * @param poseProvider provider to call to get the robot pose + * @param translationConstraints translation motion profile constraints + * @param omegaConstraints rotation motion profile constraints + */ + public DriveToPoseCommand( + CommandSwerveDrivetrain drivetrainSubsystem, + Supplier poseProvider, + TrapezoidProfile.Constraints translationConstraints, + TrapezoidProfile.Constraints omegaConstraints) { + + this.drivetrainSubsystem = drivetrainSubsystem; + this.poseProvider = poseProvider; + + xController = new ProfiledPIDController(X_kP, X_kI, X_kD, translationConstraints); + xController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + + yController = new ProfiledPIDController(Y_kP, Y_kI, Y_kD, translationConstraints); + yController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + + thetaController = new ProfiledPIDController(THETA_kP, THETA_kI, THETA_kD, omegaConstraints); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(THETA_TOLERANCE.in(Radians)); + + addRequirements(drivetrainSubsystem); + } + + /** + * Sets the goal to drive to. This should be set before the command is scheduled. + * + * @param goalPose goal pose + */ + public void setGoal(Pose2d goalPose) { + thetaController.setGoal(goalPose.getRotation().getRadians()); + xController.setGoal(goalPose.getX()); + yController.setGoal(goalPose.getY()); + } + + @Override + public void initialize() { + var robotPose = poseProvider.get(); + thetaController.reset(robotPose.getRotation().getRadians()); + xController.reset(robotPose.getX()); + yController.reset(robotPose.getY()); + } + + @Override + public void execute() { + var robotPose = poseProvider.get(); + + var xSpeed = xController.calculate(robotPose.getX()); + if (xController.atGoal()) { + xSpeed = 0; + } + + var ySpeed = yController.calculate(robotPose.getY()); + if (yController.atGoal()) { + ySpeed = 0; + } + + var omegaSpeed = thetaController.calculate(robotPose.getRotation().getRadians()); + if (thetaController.atGoal()) { + omegaSpeed = 0; + } + + drivetrainSubsystem.setControl( + fieldCentricSwerveRequest.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(omegaSpeed)); + } + + @Override + public boolean isFinished() { + return xController.atGoal() && yController.atGoal() && thetaController.atGoal(); + } + + @Override + public void end(boolean interrupted) { + drivetrainSubsystem.setControl(new SwerveRequest.Idle()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index f737cd3..6dc753f 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -98,7 +98,7 @@ public class VisionConfig { public static final Matrix STATE_STANDARD_DEVIATIONS = VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(2.5)); - public class AlignmentConfig { + public static class AlignmentConfig { public static final double THETA_kP = 3.0; public static final double THETA_kI = 0.0; public static final double THETA_kD = 0.0;