diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java index 9a19ef729ba4..0559b1126c53 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java @@ -30,11 +30,11 @@ public class Hardware extends HardwareMapper implements TriOdoProvider { public static final double SLIDE_OUTWARD_TIME = 0.45; // seconds public static final double SLIDE_OVERSHOOT = 0.05; public static final double FLIP_DOWN = 0.05; - public static final double FRONT_OPEN = 0.33; + public static final double FRONT_OPEN = 0.25; public static final double FRONT_CLOSE = 0.07; public static final double FLIP_UP = 0.98; - public static final double CLAW_CLOSE = 0.02; - public static final double CLAW_OPEN = 0.55; + public static final double CLAW_CLOSE = 0.28; + public static final double CLAW_OPEN = 0.5; public static final double WRIST_UP = 0.42; public static final double WRIST_BACK = 0.30; public static final double ARM_POWER = 0.2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java index b0117171bef5..1c1a6d4fb881 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java @@ -412,25 +412,27 @@ public void wrist() { } public void claw() { - final double open = 0.02; - final double close = 0.55; if (gamepad2.left_bumper) { abandonLock(Locks.ArmAssembly); - hardware.claw.setPosition(open); + hardware.claw.setPosition(Hardware.CLAW_OPEN); } else if (gamepad2.right_bumper) { abandonLock(Locks.ArmAssembly); - hardware.claw.setPosition(close); + hardware.claw.setPosition(Hardware.CLAW_CLOSE); } } public void resetAll() { abandonLock(Locks.ArmAssembly); abandonLock(liftProxy.CONTROL); - scheduler.add(liftProxy.moveTo(0, 5, 1.0)); - scheduler.add(groupOf(it->it.add(run(() -> { - hardware.arm.setTargetPosition(0); - hardware.wrist.setPosition(Hardware.WRIST_UP); - })).then(await(700))).extraDepends(Locks.ArmAssembly)); + scheduler.add(groupOf(it -> it.add(run(() -> hardware.claw.setPosition(Hardware.CLAW_OPEN))) + .then(await(200)) + .then(run(() -> { + hardware.arm.setTargetPosition(0); + hardware.wrist.setPosition(Hardware.WRIST_UP); + })) + .then(await(200)) + .then(liftProxy.moveTo(0, 5, 1.0)) + ).extraDepends(Locks.ArmAssembly, liftProxy.CONTROL)); } ////////////////////////////////////////////////////////////////////////////////////////////////// @@ -597,7 +599,7 @@ public void transferAndDrop() { it -> it.add(run(() -> hardware.arm.setTargetPosition(Hardware.deg2arm(35)))) .then(await(1000)) .then(run(() -> hardware.wrist.setPosition(0.75))) - .then(await(250)) + .then(await(100)) .then(run(() -> hardware.claw.setPosition(Hardware.CLAW_OPEN))) .then(await(500)) .then(run(() -> { @@ -675,7 +677,7 @@ public void transfer() { } private static class LiftProxy extends TaskTemplate { - private static final double SPEED = .75; + private static final double SPEED = 1.0; private static final int MAX_VERTICAL_LIFT_TICKS = 2300; private static final int MIN_VERTICAL_LIFT_TICKS = 0; private static final Set requires = Set.of(Locks.VerticalSlide);