From 8cee43b446f36ce90f62e0553cc3babf69c5f699 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 08:55:32 -0400 Subject: [PATCH] Adjust three piece --- .../com/stuypulse/robot/commands/auton/ThreePiece.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java index 103df410..cc2804dd 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java @@ -56,7 +56,7 @@ public AutonReady() { } }); } - + @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); @@ -65,7 +65,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) .setWristTolerance(45)) .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) - .setWristTolerance(30).setShoulderTolerance(20)); + .setWristTolerance(7).setShoulderTolerance(25)); } } static class ArmIntakeFirst extends ArmRoutine { @@ -174,7 +174,7 @@ public ThreePiece() { addCommands( new LEDSet(LEDColor.BLUE), new IntakeScore(), - new WaitCommand(INTAKE_DEACQUIRE_TIME) + new WaitCommand(0.8) ); // intake second piece @@ -188,7 +188,8 @@ public ThreePiece() { .withStop(), // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early - new WaitCommand(INTAKE_STOP_WAIT_TIME) + new IntakeScore() + .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) .andThen(new IntakeAcquire()),