From 2720d596b597c6c01bd2f19f555b806d95faf375 Mon Sep 17 00:00:00 2001 From: carlos Date: Fri, 13 Dec 2024 16:42:29 -0800 Subject: [PATCH] Blue basket auto comp 2 updates --- .../ftc/teamcode/BlueBasketAuto.java | 192 +++--------------- .../ftc/teamcode/BlueBasketAutoWithDrop3.java | 8 +- .../ftc/teamcode/BlueBasketAutoWithDrop4.java | 8 +- .../ftc/teamcode/BlueBasketAutoWithDrop5.java | 8 +- .../ftc/teamcode/BlueBasketAutoWithDrop6.java | 9 +- .../cometbots/CometBotTeleopCompetition.java | 48 +++-- .../subsystem/ClawActionsSubsystem.java | 2 +- .../subsystem/HighBasketAutoPath1.java | 39 ++++ .../subsystem/HighBasketAutoPath2.java | 36 ++++ 9 files changed, 149 insertions(+), 201 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.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 e65fb51..0534aec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java @@ -3,9 +3,12 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleopCompetition; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; @@ -13,184 +16,47 @@ 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; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1; +import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2; +@Disabled @Autonomous(name = "Auto Test", group = "Dev") public class BlueBasketAuto extends OpMode { - private Telemetry telemetryA; - private Follower follower; + private int state; - private PathChain path; - - private final Pose startPose = new Pose(8, 65); + private HighBasketAutoPath1 path1; + private HighBasketAutoPath2 path2; + private CometBotTeleopCompetition comp; + private ElapsedTime runtime; @Override public void init() { follower = new Follower(hardwareMap); - follower.setMaxPower(.75); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierLine( - new Point(8.000, 65.000, Point.CARTESIAN), - new Point(33.000, 65.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 2 - new BezierLine( - new Point(33.000, 65.000, Point.CARTESIAN), - new Point(31.000, 65.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 3 - new BezierCurve( - new Point(31.000, 65.000, Point.CARTESIAN), - new Point(26.000, 32.000, Point.CARTESIAN), - new Point(60.000, 34.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) - .addPath( - // Line 4 - new BezierLine( - new Point(60.000, 34.000, Point.CARTESIAN), - new Point(60.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 5 - new BezierLine( - new Point(60.000, 24.000, Point.CARTESIAN), - new Point(14.500, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 6 - new BezierLine( - new Point(14.500, 24.000, Point.CARTESIAN), - new Point(18.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 1 - new BezierCurve( - new Point(18.000, 24.000, Point.CARTESIAN), - new Point(18.000, 67.000, Point.CARTESIAN), - new Point(31.000, 67.000, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 8 - new BezierLine( - new Point(31.000, 67.500, Point.CARTESIAN), - new Point(33.000, 67.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 9 - new BezierLine( - new Point(33.000, 67.500, Point.CARTESIAN), - new Point(31.000, 67.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 10 - new BezierCurve( - new Point(31.000, 67.500, Point.CARTESIAN), - new Point(26.000, 32.000, Point.CARTESIAN), - new Point(60.000, 34.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) - .addPath( - // Line 11 - new BezierLine( - new Point(60.000, 34.000, Point.CARTESIAN), - new Point(60.000, 12.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 12 - new BezierLine( - new Point(60.000, 12.000, Point.CARTESIAN), - new Point(12.000, 12.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 13 - new BezierLine( - new Point(12.000, 12.000, Point.CARTESIAN), - new Point(20.000, 12.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 14 - new BezierLine( - new Point(20.000, 12.000, Point.CARTESIAN), - new Point(20.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 15 - new BezierLine( - new Point(20.000, 24.000, Point.CARTESIAN), - new Point(14.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 16 - new BezierCurve( - new Point(14.000, 24.000, Point.CARTESIAN), - new Point(14.000, 70.000, Point.CARTESIAN), - new Point(31.000, 70.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0)) - .addPath( - // Line 17 - new BezierLine( - new Point(31.000, 70.000, Point.CARTESIAN), - new Point(33.000, 70.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 18 - new BezierLine( - new Point(33.000, 70.000, Point.CARTESIAN), - new Point(31.000, 70.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build(); - follower.followPath(path); - - telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - telemetryA.update(); + path1 = new HighBasketAutoPath1(); + path2 = new HighBasketAutoPath2(); + comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); + comp.initCloseClaw(); } @Override public void loop() { + switch(state) { + case 0: + path1.moveToPath1(follower); + state = 1; + runtime.reset(); + case 1: + if (runtime.seconds() > 5) { + path2.moveToPath1(follower); + state = 2; + } + default: + System.out.println("default"); + } follower.update(); - follower.telemetryDebug(telemetryA); + follower.telemetryDebug(telemetry); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java index 04ad369..caeab2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -11,8 +12,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -@Autonomous(name = "Auto Test", group = "Dev") +@Disabled +@Autonomous(name = "DKJBFWOFHBOUW EHBF", group = "Dev") public class BlueBasketAutoWithDrop3 extends OpMode { private Telemetry telemetryA; @@ -20,7 +21,6 @@ public class BlueBasketAutoWithDrop3 extends OpMode { private PathChain path; - private final Pose startPose = new Pose(8, 65); @Override public void init() { @@ -28,8 +28,6 @@ public class BlueBasketAutoWithDrop3 extends OpMode { follower.setMaxPower(.75); - follower.setStartingPose(startPose); - path = follower.pathBuilder() .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) .addPath( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java index a68d354..5cdf963 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -12,8 +13,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -@Autonomous(name = "Auto Test", group = "Dev") +@Disabled +@Autonomous(name = "rndnohsdnhngph", group = "Dev") public class BlueBasketAutoWithDrop4 extends OpMode { private Telemetry telemetryA; @@ -21,7 +22,6 @@ public class BlueBasketAutoWithDrop4 extends OpMode { private PathChain path; - private final Pose startPose = new Pose(8, 65); @Override public void init() { @@ -29,8 +29,6 @@ public class BlueBasketAutoWithDrop4 extends OpMode { follower.setMaxPower(.75); - follower.setStartingPose(startPose); - path = follower.pathBuilder() .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) .addPath( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java index f8edce6..529c3f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -12,8 +13,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -@Autonomous(name = "Auto Test", group = "Dev") +@Disabled +@Autonomous(name = "rboeruhbgoeruhbg", group = "Dev") public class BlueBasketAutoWithDrop5 extends OpMode { private Telemetry telemetryA; @@ -21,7 +22,6 @@ public class BlueBasketAutoWithDrop5 extends OpMode { private PathChain path; - private final Pose startPose = new Pose(8, 65); @Override public void init() { @@ -29,8 +29,6 @@ public class BlueBasketAutoWithDrop5 extends OpMode { follower.setMaxPower(.75); - follower.setStartingPose(startPose); - path = follower.pathBuilder() .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) .addPath( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java index 90bc3c3..17939d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -11,8 +12,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -@Autonomous(name = "Auto Test", group = "Dev") +@Disabled +@Autonomous(name = "piqeripgqreipbripbipb", group = "Dev") public class BlueBasketAutoWithDrop6 extends OpMode { private Telemetry telemetryA; @@ -20,16 +21,12 @@ public class BlueBasketAutoWithDrop6 extends OpMode { private PathChain path; - private final Pose startPose = new Pose(8, 65); - @Override public void init() { follower = new Follower(hardwareMap); follower.setMaxPower(.75); - follower.setStartingPose(startPose); - path = follower.pathBuilder() .addPath( // Line 6 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 eac0db5..7cf501d 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 @@ -88,6 +88,18 @@ public class CometBotTeleopCompetition { follower.startTeleopDrive(); } + public void initCloseClaw(){ + this.motors.init(); + this.hook.init(); + this.claw.init(); + this.arm.init(); + this.wrist.init(); + this.lift.init(); + this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + follower.setMaxPower(MAX_POWER); + follower.startTeleopDrive(); + } + public void update() { this.previousGP1.copy(currentGP1); this.currentGP1.copy(this.GP1); @@ -162,28 +174,32 @@ public class CometBotTeleopCompetition { if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) { this.follower.breakFollowing(); fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); - Actions.runBlocking(new SequentialAction( - new SleepAction(.5), - this.lift.toHighBucketPosition(), - new SleepAction(.5), - this.arm.toBucketPosition(), - new SleepAction(.5), - this.wrist.toBucketPosition(), - new SleepAction(.5), - this.claw.openClaw(), - new SleepAction(.5), - this.wrist.toFloorPosition(), - new SleepAction(.5), - this.arm.toParkPosition(), - this.lift.toFloorPosition(), - new SleepAction(.5) - )); + highBucketDrop(); fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); this.follower.startTeleopDrive(); } } } + public void highBucketDrop() { + Actions.runBlocking(new SequentialAction( + new SleepAction(.5), + this.lift.toHighBucketPosition(), + new SleepAction(.5), + this.arm.toBucketPosition(), + new SleepAction(.5), + this.wrist.toBucketPosition(), + new SleepAction(.5), + this.claw.openClaw(), + new SleepAction(.5), + this.wrist.toFloorPosition(), + new SleepAction(.5), + this.arm.toParkPosition(), + this.lift.toFloorPosition(), + new SleepAction(.5) + )); + } + /* Type: PS4 / Logitech Controller: 2 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java index d3a2d8b..dbbc1c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawActionsSubsystem.java @@ -68,7 +68,7 @@ public class ClawActionsSubsystem { } public void init() { - Actions.runBlocking(openClaw()); + Actions.runBlocking(closeClaw()); } public void start() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java new file mode 100644 index 0000000..2df4651 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java @@ -0,0 +1,39 @@ +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.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 HighBasketAutoPath1 { + + private final Pose startPose = new Pose(8, 89); + + public void moveToPath1(Follower robot) { + PathChain pathChain; + robot.setStartingPose(startPose); + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 1 + new BezierCurve( + new Point(8.000, 89.000, Point.CARTESIAN), + new Point(24.000, 96.000, Point.CARTESIAN), + new Point(16.000, 128.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/HighBasketAutoPath2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.java new file mode 100644 index 0000000..a956d4f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath2.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.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; +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 HighBasketAutoPath2 { + + public void moveToPath1(Follower robot) { + PathChain pathChain; + PathBuilder builder = new PathBuilder(); + builder + .addPath( + // Line 2 + new BezierCurve( + new Point(16.000, 128.000, Point.CARTESIAN), + new Point(16.000, 120.000, Point.CARTESIAN), + new Point(32.000, 123.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)); + pathChain = builder.build(); + robot.followPath(pathChain); + } + +} +