From 71b91fa3ff48cb54d63e6738856a7cd8d5dd378e Mon Sep 17 00:00:00 2001 From: robotics1 Date: Wed, 22 Jan 2025 12:31:31 -0800 Subject: [PATCH] random --- .../cometbots/paths/CometBotDriveV2.java | 36 +++++++++---------- .../cometbots/paths/HighBasketPath3.java | 35 ++++++++++++++++++ .../ftc/teamcode/subsystem/ClawSubsystem.java | 4 +-- .../subsystem/DualMotorSliderSubsystem.java | 6 +++- .../ftc/teamcode/subsystem/LiftArmWrist.java | 3 +- 5 files changed, 61 insertions(+), 23 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java index c24f3f8..f810a65 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java @@ -87,21 +87,21 @@ public class CometBotDriveV2 extends OpMode { switch (state) { case 0: moveToPathOneAndHighBucket(); -// doArmThing1(); break; case 1: -// doArmThing2(); + doArmThing(); state = 2; break; case 2: moveToPathTwoAndPickSampleUp(); break; -// case 3: + case 3: // doPickUpThing(); -// break; -// case 4: -// moveToBasketPath3(); -// break; + state = 4; + break; + case 4: + // moveToBasketPath3(); + break; // case 5: // theArmThing(); // break; @@ -154,19 +154,17 @@ public class CometBotDriveV2 extends OpMode { return new SetStateAction(value); } - private void doArmThing1() { - lift.toHighBucketPosition(); + private void doArmThing() { + + lift.toHighBucketReverseDrop(); arm.toBucketPosition(); + wrist.toBucketPosition(); + claw.openClaw(); + wrist.toFloorPosition(); state = 2; } - private void doArmThing2() { - wrist.toBucketPosition(); - claw.openClaw(); - wrist.toFloorPosition(); - state = 2; - } // private void theArmThing() { // telemetry.addData("busy?", follower.isBusy()); @@ -192,12 +190,12 @@ public class CometBotDriveV2 extends OpMode { } } // - private void moveToPathBasketPath3() { + private void moveToBasketPath3() { if (!follower.isBusy()) { path3.moveToBasketPath3(follower); - state = 3; - - } } + state = 5; + } + } // private void moveToPickupAgainPath4() { // if (!followingPath) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java new file mode 100644 index 0000000..c2ebd55 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath3.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + + +/* + AutoLine# - This file does something of a path...... + + */ +public class HighBasketPath3 { + + + public void moveToBasketPath3(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(26.000, 117.000, Point.CARTESIAN), + new Point(10.000, 89.00, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java index 16f186e..af1c2ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java @@ -34,13 +34,13 @@ public class ClawSubsystem { } public void closeClaw() { claw.setPosition(clawClose); - thumbDown(); + state = ClawState.CLOSED; } public void openClaw() { claw.setPosition(clawOpen); - thumbUp(); + state = ClawState.OPEN; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java index 7172afa..2a43bd5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; @@ -71,7 +72,9 @@ public class DualMotorSliderSubsystem { liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR); liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR); } - + public int getCurrentPosition(){ + return liftSlideLeft.getCurrentPosition(); + } public void init() { /* Initialize the motors with the following settings (assuming slide is at the very bottom position): @@ -141,5 +144,6 @@ public class DualMotorSliderSubsystem { } public void toFloorPosition(){setTargetPosition(0);} + public void toHighBucketReverseDrop(){setTargetPosition(backwardBucketDrop);} } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java index dfde281..b07e06c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java @@ -108,6 +108,7 @@ public class LiftArmWrist extends LinearOpMode { wrist.setPosition(wrist.getPosition() - .05); } + if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) { lift.toHighBucketPosition(); } @@ -124,7 +125,7 @@ public class LiftArmWrist extends LinearOpMode { telemetry.addData("Lift Drive Position", lift.getFixedPosition()); telemetry.addData("Wrist Position", wrist.getPosition()); telemetry.addData("Arm Position", arm.getPosition()); - + telemetry.addData("Slide Ticks", lift.getCurrentPosition()); telemetry.update(); } }