From 70f5051553a907a3f63e168fea23603cc3f9f269 Mon Sep 17 00:00:00 2001 From: ryan C <147014849+luenix1@users.noreply.github.com> Date: Sat, 11 Jan 2025 21:13:53 -0800 Subject: [PATCH] readjusted horizontal slides and added saftey for the twist --- .../firstinspires/ftc/teamcode/Hardware.java | 9 +++-- .../ftc/teamcode/MecanumTeleOp.java | 22 ++++++++++-- .../ftc/teamcode/MecanumTeleOp2.java | 35 ++++++++++++++++++- .../ftc/teamcode/ServoSteper.java | 3 +- 4 files changed, 62 insertions(+), 7 deletions(-) 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 3eadc4367bc5..ebe7f793845a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java @@ -22,10 +22,11 @@ public class Hardware extends HardwareMapper implements TriOdoProvider { public static final double spinTickPerRev = 751.8; - public static final double RIGHT_SLIDE_OUT = 0.72; + public static final double RIGHT_SLIDE_OUT = 0.64; @Deprecated public static final double LEFT_SLIDE_OUT = 1 - RIGHT_SLIDE_OUT; - public static final double RIGHT_SLIDE_IN = 0.45; + public static final double RIGHT_SLIDE_IN = 0.4; @Deprecated public static final double LEFT_SLIDE_IN = 1 - RIGHT_SLIDE_IN; + public static final double CLAW_TWIST_INIT = 0.48; public static final double SLIDE_INWARD_TIME = 0.75; // seconds public static final double SLIDE_OUTWARD_TIME = 0.45; // seconds public static final double SLIDE_OVERSHOOT = 0.05; @@ -138,6 +139,9 @@ public static class Locks { @HardwareName("clawFlip") public Servo clawFlip; + @HardwareName("clawTwist") + public Servo clawTwist; + @HardwareName("horizontalSlide") public Servo horizontalSlide; @@ -153,6 +157,7 @@ public static class Locks { @HardwareName("clawColor") public ColorSensor clawColor; + @Override public Encoder getLeftEncoder() { return encoderLeft; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java index 2707c885c0ef..7f5c8c322e5b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java @@ -24,6 +24,9 @@ public class MecanumTeleOp extends LinearOpMode { double VerticalSlideSpeed = 0.75; double ClawFrontPos = 0.5; double ClawFlipPos = 1.0; + double TwistINIT = 0.48; + + @Override public void runOpMode() { @@ -39,6 +42,7 @@ public void runOpMode() { hardware.arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); hardware.arm.setPower(Hardware.ARM_POWER); hardware.wrist.setPosition(0.28); + hardware.clawTwist.setPosition(TwistINIT); IntegratingGyroscope gyro; NavxMicroNavigationSensor navxMicro; ElapsedTime timer = new ElapsedTime(); @@ -63,6 +67,7 @@ public void runOpMode() { double yaw_offset = 0.0; while (opModeIsActive()) { + Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS); if (gamepad1.back) { yaw_offset = angles.firstAngle; @@ -156,6 +161,7 @@ String formatDegrees(double degrees) { int highChamberTicks = 790; int highBasketTicks = 2180; + // lifts the vertical slides to a target position in ticks private void targetLift(Hardware hardware, int targetPosition) { @@ -362,7 +368,7 @@ private void stepper(Hardware hardware) { ClawFlipPos = 1.0; } } - hardware.clawFlip.setPosition(ClawFlipPos); + hardware.clawTwist.setPosition(ClawFrontPos); hardware.clawFront.setPosition(ClawFrontPos); // clawFront close is 0 //clawFront open is 0.27 @@ -445,6 +451,11 @@ public void Horizontalpick(Hardware hardware) { } public void Flipout(Hardware hardware) { + double TwistPos = hardware.clawTwist.getPosition(); + boolean FlipNotSafe = TwistPos != TwistINIT; + if (FlipNotSafe){ + hardware.clawTwist.setPosition(TwistINIT); + } hardware.horizontalSlide.setPosition(Hardware.RIGHT_SLIDE_OUT); hardware.horizontalLeft.setPosition(Hardware.LEFT_SLIDE_OUT); sleep(500); @@ -454,6 +465,11 @@ public void Flipout(Hardware hardware) { } public void Flipin(Hardware hardware) { + double TwistPos = hardware.clawTwist.getPosition(); + boolean FlipNotSafe = TwistPos != TwistINIT; + if (FlipNotSafe){ + hardware.clawTwist.setPosition(TwistINIT); + } double fliponethird = 0.66; hardware.clawFlip.setPosition(fliponethird); ClawFlipPos = fliponethird; @@ -467,7 +483,7 @@ public void Flipin(Hardware hardware) { boolean lastX = false; - public void trasfer(Hardware hardware){ + public void trasfer(Hardware hardware) { boolean x = gamepad2.x; if (x && !lastX) { hardware.clawFront.setPosition(Hardware.FRONT_CLOSE); @@ -484,7 +500,7 @@ public void trasfer(Hardware hardware){ hardware.clawFront.setPosition(Hardware.FRONT_OPEN); sleep(500); hardware.arm.setTargetPosition(0); - armTargetPosDeg=0; + armTargetPosDeg = 0; sleep(500); hardware.wrist.setPosition(0.28); ClawFrontPos = Hardware.FRONT_OPEN; 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 93453e7e26b1..bc8ba39e637f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp2.java @@ -117,7 +117,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); @@ -237,6 +237,7 @@ public void runOpMode() { stepper(); lift(); arm(); + TwistStepper(); boolean shouldScoreHigh = gamepad2.left_trigger > 0.5; boolean shouldScoreHigh2 = gamepad2.right_trigger > 0.5; @@ -592,6 +593,8 @@ public void transferAndDrop() { } public void Flipout() { + double TwistPos = hardware.clawTwist.getPosition(); + boolean FlipNotSafe = TwistPos != Hardware.CLAW_TWIST_INIT; abandonLock(hSlideProxy.CONTROL); abandonLock(hClawProxy.CONTROL_CLAW); abandonLock(hClawProxy.CONTROL_FLIP); @@ -607,6 +610,8 @@ public void Flipout() { } public void Flipin() { + double TwistPos = hardware.clawTwist.getPosition(); + boolean FlipNotSafe = TwistPos != Hardware.CLAW_TWIST_INIT; abandonLock(hSlideProxy.CONTROL); abandonLock(hClawProxy.CONTROL_FLIP); double flipThird = 0.66; @@ -655,5 +660,33 @@ public void transfer() { abandonLock(Locks.ArmAssembly); transferInternal(); } + private void TwistStepper() { + double Position = 0.5; + if (gamepad1.dpad_left) { + Position = Hardware.FRONT_OPEN; + } + if (gamepad1.dpad_right) { + Position = Hardware.FRONT_CLOSE; + } + if (gamepad1.dpad_down) { + Position -= 0.01; + if(Position<0){ + Position = 0.0; + } + } + if (gamepad1.dpad_up) { + Position += 0.01; + if(Position>1){ + Position = 1.0; + } + } + hardware.clawTwist.setPosition(Position); + hardware.clawFront.setPosition(Position); + // clawFront close is 0 + //clawFront open is 0.27 + + telemetry.addData("Pos", Position); + telemetry.addData("Pos", Position); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoSteper.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoSteper.java index 1174ccff55de..04338946f375 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoSteper.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoSteper.java @@ -73,7 +73,8 @@ public void runOpMode() { // Set the servo to the new position and pause; sleep(CYCLE_MS); idle(); - hardware.claw.setPosition(position); + hardware.horizontalSlide.setPosition(position); + hardware.horizontalLeft.setPosition(1-position); }