From a39332954ef110b1c6c776194588caedb028e574 Mon Sep 17 00:00:00 2001 From: carlos Date: Tue, 14 Jan 2025 15:39:17 -0800 Subject: [PATCH] Fixed floor position issue --- .../ftc/teamcode/BlueBasketAuto.java | 4 +- .../ftc/teamcode/ComeBotDriveDevV2.java | 102 +++++++++++++++++- .../cometbots/CometBotTeleopCompetition.java | 14 ++- .../ftc/teamcode/configs/RobotConstants.java | 4 +- .../subsystem/HighBasketAutoPath2.java | 12 +-- .../subsystem/HighBasketAutoPath3.java | 36 +++++++ .../subsystem/HighBasketAutoPath4.java | 34 ++++++ 7 files changed, 193 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java index f995c3f..7dac8c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java @@ -78,8 +78,8 @@ public class BlueBasketAuto extends OpMode { if (runtime.seconds() > 15) { telemetry.addData("case2", "case2"); - new SleepAction(.5); - path2.moveToPath1(follower); + // new SleepAsction(.5); + //path2.moveToPath1(follower); //For next time, add encoder control to skyhook and extend here //comp.moveSkyHook(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java index 0162aa3..aaa0222 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java @@ -4,6 +4,9 @@ import androidx.annotation.NonNull; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.util.ElapsedTime; @@ -13,6 +16,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.subsystem.AutoPark; import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1; import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; @@ -24,6 +29,10 @@ public class ComeBotDriveDevV2 extends OpMode { private HighBasketAutoPath1 path1; private HighBasketAutoPath2 path2; + + private HighBasketAutoPath3 path3; + private HighBasketAutoPath4 path4; + private AutoPark pathPark; private SkyHookSubsystem hook; private CometBotTeleopCompetition comp; @@ -60,6 +69,33 @@ public class ComeBotDriveDevV2 extends OpMode { case 2: moveToPathTwoAndPickSampleUp(); break; + case 3: + doPickUpThing(); + break; + case 4: + moveToBasketPath3(); + break; + case 5: + theArmThing(); + break; + case 6: + moveToPickupAgainPath4(); + break; + case 7: + doPickUpThingAgain(); + break; + case 8: + //line five + break; + case 9: + theArmThingAgain(); + break; + case 10: + //line six + break; + case 11: + + } telemetry.update(); follower.update(); @@ -74,7 +110,7 @@ public class ComeBotDriveDevV2 extends OpMode { if (runtime != null) { telemetry.addData("Runtime (seconds)", runtime.seconds()); if (follower.atParametricEnd() || runtime.seconds() > 6.0) { - state = 1; + state = 3; followingPath = false; } } @@ -104,6 +140,16 @@ public class ComeBotDriveDevV2 extends OpMode { state = 2; } + private void theArmThing(){ + comp.highBucketDrop(); + state = 6; + } + + private void theArmThingAgain(){ + comp.highBucketDrop(); + state = 10; + } + private void moveToPathTwoAndPickSampleUp() { if (!followingPath) { path2.moveToPath2(follower); @@ -111,12 +157,64 @@ public class ComeBotDriveDevV2 extends OpMode { } if (runtime != null) { telemetry.addData("Runtime (seconds)", runtime.seconds()); - if (follower.atParametricEnd() || runtime.seconds() > 12.0) { + if (follower.atParametricEnd() || runtime.seconds() > 22.0) { state = 3; followingPath = false; } } } + private void moveToPickupAgainPath4() { + if (!followingPath) { + path4.moveToPickupAgainPath4(follower); + followingPath = true; + } + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + if (follower.atParametricEnd() || runtime.seconds() > 12.0) { + state = 7; + followingPath = false; + } + } + } + private void moveToBasketPath3() { + if (!followingPath) { + path3.moveToBasketPath3(follower); + followingPath = true; + } + if (runtime != null) { + telemetry.addData("Runtime (seconds)", runtime.seconds()); + if (follower.atParametricEnd() || runtime.seconds() > 12.0) { + state = 5; + followingPath = false; + } + } + } + + private void thePickUp(){ + Actions.runBlocking(new SequentialAction( + comp.arm.toFloorPosition(), + new SleepAction(.5), + comp.wrist.toPickupPosition(), + new SleepAction(1), + comp.claw.closeClaw(), + new SleepAction(1), + comp.wrist.toFloorPosition(), + new SleepAction(0.5), + comp.arm.toParkPosition() + )); + } + + private void doPickUpThing(){ + thePickUp(); + if(!follower.isBusy()) { + state = 4; + } + } + + private void doPickUpThingAgain(){ + thePickUp(); + state = 8; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java index 4b706ac..10b2658 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java @@ -105,7 +105,7 @@ public class CometBotTeleopCompetition { this.currentGP1.copy(this.GP1); this.previousGP2.copy(currentGP2); this.currentGP2.copy(this.GP2); - + this.moveSkyHook(); this.toHighBucketScore(); this.toLowBucketScore(); this.toArmParkPosition(); @@ -157,6 +157,18 @@ public class CometBotTeleopCompetition { this.follower.setMaxPower(this.currentPower); } } + public void moveSkyHook() { + if (this.currentGP2.dpad_down) { + hook.moveSkyHook(-1.00); + } + else if (this.currentGP2.dpad_up) { + hook.moveSkyHook(1.00); + } + else{ + hook.moveSkyHook(0.00); + } + + } /* Type: PS4 / Logitech diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 3936e3a..190220b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { public final static double clawClose = 0.8; - public final static double clawOpen = 0.05; + public final static double clawOpen = 0.15; public final static double armFloor = 0.7; public final static double armSubmarine = 0.55; @@ -20,7 +20,7 @@ public class RobotConstants { public final static double wristSpeciemen = 0.1; - public final static int liftToFloorPos = 550; + public final static int liftToFloorPos = 500; public final static int liftToLowBucketPos = 2650; public final static int liftToHighRung = 2100; public final static int dropToHighRung = 1675; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java index 50f70c8..1f52aac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java @@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +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; @@ -15,19 +16,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; */ public class HighBasketAutoPath2 { - public void moveToPath1(Follower robot) { + public void moveToPath2(Follower robot) { PathChain pathChain; PathBuilder builder = new PathBuilder(); builder .addPath( - // Line 2 - new BezierCurve( + // Line 1 + new BezierLine( new Point(18.000, 126.000, Point.CARTESIAN), - new Point(85.000, 132.750, Point.CARTESIAN), - new Point(84.000, 97.000, Point.CARTESIAN) + new Point(29.000, 120.000, Point.CARTESIAN) ) ) - .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135)); + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)); pathChain = builder.build(); robot.followPath(pathChain); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java new file mode 100644 index 0000000..72b9405 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.subsystem; + + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +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 HighBasketAutoPath3 { + + public void moveToBasketPath3(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(29.000, 120.000, Point.CARTESIAN), + new Point(18.000, 126.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)); + + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java new file mode 100644 index 0000000..490efd0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.subsystem; + + +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 HighBasketAutoPath4 { + + public void moveToPickupAgainPath4(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierLine( + new Point(18.000, 126.000, Point.CARTESIAN), + new Point(29.000, 131.400, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} +