diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 9703625..4525a6c 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -166,9 +166,9 @@ public static Command alignToReef(Drive drive, boolean alignLeft) { yController.reset(); thetaController.reset(); - xController.setTolerance(Units.inchesToMeters(0.1)); - yController.setTolerance(Units.inchesToMeters(0.1)); - thetaController.setTolerance(Units.degreesToRadians(1)); + xController.setTolerance(Units.inchesToMeters(0.5)); + yController.setTolerance(Units.inchesToMeters(0.5)); + thetaController.setTolerance(Units.degreesToRadians(2)); return Commands.run( () -> { diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index c098da6..8962327 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -9,42 +9,42 @@ public class SuperstructureCommands { public static Command scoreRightL4Sim(Drive drive, SwerveDriveSimulation driveSimulation) { return Commands.sequence( - Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)), + Commands.race(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)), Commands.waitSeconds(0.1), IntakeCommands.scoreL4Sim(driveSimulation)); } public static Command scoreLeftL4Sim(Drive drive, SwerveDriveSimulation driveSimulation) { return Commands.sequence( - Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)), + Commands.race(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)), Commands.waitSeconds(0.1), IntakeCommands.scoreL4Sim(driveSimulation)); } public static Command scoreRightL3Sim(Drive drive, SwerveDriveSimulation driveSimulation) { return Commands.sequence( - Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)), + Commands.race(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)), Commands.waitSeconds(0.1), IntakeCommands.scoreL3Sim(driveSimulation)); } public static Command scoreLeftL3Sim(Drive drive, SwerveDriveSimulation driveSimulation) { return Commands.sequence( - Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)), + Commands.race(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)), Commands.waitSeconds(0.1), IntakeCommands.scoreL3Sim(driveSimulation)); } public static Command scoreRightL2Sim(Drive drive, SwerveDriveSimulation driveSimulation) { return Commands.sequence( - Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)), + Commands.race(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)), Commands.waitSeconds(0.1), IntakeCommands.scoreL2Sim(driveSimulation)); } public static Command scoreLeftL2Sim(Drive drive, SwerveDriveSimulation driveSimulation) { return Commands.sequence( - Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)), + Commands.race(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)), Commands.waitSeconds(0.1), IntakeCommands.scoreL2Sim(driveSimulation)); }