diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FixStuffTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FixStuffTeleOp.java index 3d4289f47962..c1bd2b7787e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FixStuffTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FixStuffTeleOp.java @@ -61,7 +61,7 @@ private void hardwareInit() { hardware.arm.setPower(0.3); hardware.wrist.setPosition(0.28); hardware.claw.setPosition(Hardware.CLAW_CLOSE); - + hardware.clawTwist.setPosition(Hardware.CLAW_TWIST_INIT); // we don't have the proxy object to handle this for us // so manually implement the inversion hardware.horizontalSlide.setPosition(Hardware.RIGHT_SLIDE_IN); @@ -88,6 +88,9 @@ public void runOpMode() { if (gamepad1.b){ SlideIn(); } + if (gamepad1.x){ + FourthSample(); + } telemetry.addData("slidePos", hardware.horizontalLeft.getPosition()); telemetry.addData("slidePos2", hardware.horizontalSlide.getPosition()); @@ -114,4 +117,27 @@ public void SlideIn() { hardware.horizontalLeft.setPosition(1-Hardware.RIGHT_SLIDE_IN); hardware.clawFlip.setPosition(Hardware.FLIP_UP); } + public void FourthSample(){ + double PartialFlip = 0.167; + hardware.clawFlip.setPosition(PartialFlip); + sleep(500); + hardware.clawTwist.setPosition(0.26); + sleep(500); + hardware.horizontalSlide.setPosition(Hardware.RIGHT_SLIDE_OUT); + hardware.horizontalLeft.setPosition(1-Hardware.RIGHT_SLIDE_OUT); + sleep(500); + hardware.clawFlip.setPosition(Hardware.FLIP_DOWN); + sleep(500); + hardware.clawFront.setPosition(Hardware.FRONT_CLOSE); + sleep(500); + hardware.clawTwist.setPosition(Hardware.CLAW_TWIST_INIT); + sleep(500); + hardware.horizontalSlide.setPosition(Hardware.SLIDE_OVERSHOOT); + hardware.horizontalLeft.setPosition(1 - Hardware.SLIDE_OVERSHOOT); + sleep(500); + hardware.horizontalSlide.setPosition(Hardware.RIGHT_SLIDE_IN); + hardware.horizontalLeft.setPosition(1-Hardware.RIGHT_SLIDE_IN); + sleep(500); + hardware.clawFlip.setPosition(Hardware.FLIP_UP); + } } 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 87d9df3429db..f5abdb29b814 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java @@ -141,6 +141,9 @@ public static class Locks { @HardwareName("clawFlip") public Servo clawFlip; + @HardwareName("clawTwist") + public Servo clawTwist; + @HardwareName("horizontalSlide") public Servo horizontalSlide; @@ -156,6 +159,7 @@ public static class Locks { @HardwareName("clawColor") public ColorSensor clawColor; + @Override public Encoder getLeftEncoder() { return encoderLeft;