From 6695140d0409aa21c54f8db55dd7f8ec02e1592a Mon Sep 17 00:00:00 2001 From: robotics1 Date: Tue, 10 Dec 2024 16:48:14 -0800 Subject: [PATCH] Working actions files sky hook and more --- .../ftc/teamcode/BlueBasketAutoWithDrop.java | 195 ++++++++++++++++++ .../firstinspires/ftc/teamcode/NetAuto.java | 6 +- .../ftc/teamcode/PedroConstants.java | 16 +- .../cometbots/CometBotAutoDevelopment.java | 3 - .../cometbots/CometBotTeleopCompetition.java | 6 +- .../cometbots/tests/SpecimenTest.java | 52 ++--- .../ftc/teamcode/configs/RobotConstants.java | 6 +- .../ftc/teamcode/subsystem/AutoLine1.java | 61 +----- .../ftc/teamcode/subsystem/AutoLine2.java | 40 ++++ .../teamcode/subsystem/SkyHookSubsystem.java | 3 +- 10 files changed, 281 insertions(+), 107 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java new file mode 100644 index 0000000..421c620 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java @@ -0,0 +1,195 @@ +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.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +@Autonomous(name = "Auto Test", group = "Dev") +public class BlueBasketAutoWithDrop extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + 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 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(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java index ad61f4e..a25b6e1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java @@ -18,14 +18,10 @@ public class NetAuto extends OpMode { follower = new Follower(hardwareMap); } - public Action autoLine1() {return new AutoLine1(follower);} - @Override public void loop() { follower.update(); - Actions.runBlocking( - autoLine1() - ); + follower.telemetryDebug(telemetry); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index b8c2ef2..48428ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -10,10 +10,10 @@ public class PedroConstants { /* Motor configuration names */ - public static final String FRONT_LEFT_MOTOR = "Drive front lt"; - public static final String BACK_LEFT_MOTOR = "Drive back lt"; - public static final String FRONT_RIGHT_MOTOR = "Drive front rt"; - public static final String BACK_RIGHT_MOTOR = "Drive back rt"; + public static final String FRONT_LEFT_MOTOR = "front-left"; + public static final String BACK_LEFT_MOTOR = "back-left"; + public static final String FRONT_RIGHT_MOTOR = "front-right"; + public static final String BACK_RIGHT_MOTOR = "back-right"; /* Motor directions @@ -31,7 +31,7 @@ public class PedroConstants { /* Motor Max Power */ - public static final double MAX_POWER = .50; + public static final double MAX_POWER = .675; /* IMU @@ -45,9 +45,9 @@ public class PedroConstants { /* Dead wheels */ - public static final String LEFT_ENCODER = "encoder left"; - public static final String RIGHT_ENCODER = "encoder right"; - public static final String BACK_ENCODER = "encoder back"; + public static final String RIGHT_ENCODER = "back-right"; + public static final String BACK_ENCODER = "front-right"; + public static final String LEFT_ENCODER = "front-left"; /* Dead wheel directions diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java index b650d47..41ea54b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java @@ -99,9 +99,6 @@ public class CometBotAutoDevelopment { this.telemetry.addData("Lift Position", this.lift.getPosition()); } - public Action autoLine1() {return new AutoLine1(follower);} - - /* Controller: 1 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 22cb7f7..4aefd4d 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 @@ -294,15 +294,15 @@ public class CometBotTeleopCompetition { } } - public void raiseSkyHook() { + public void lowerSkyHook() { hook.raiseHook(currentGP2.left_trigger); } - public void lowerSkyHook() { - hook.lowerHook(currentGP2.right_trigger); + public void raiseSkyHook() { + hook.lowerHook(currentGP2.right_trigger * 2.0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java index ef68e88..9141776 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java @@ -48,6 +48,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.AutoLine1; +import org.firstinspires.ftc.teamcode.subsystem.AutoLine2; import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem; @@ -60,22 +62,14 @@ public class SpecimenTest extends LinearOpMode { private PathChain path; + private Follower robot; + private final Pose startPose = new Pose(36, 72); - public class MoveToPath implements Action { - private Follower actionRobot; - private PathChain pathChain; - private MoveToPath(PathChain path, Follower robot) { - this.actionRobot = robot; - this.pathChain = path; - } - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - return false; - } - } + + public AutoLine1 firstPath = new AutoLine1(); @Override public void runOpMode() { @@ -89,16 +83,17 @@ public class SpecimenTest extends LinearOpMode { ArmActionsSubsystem arm = new ArmActionsSubsystem(hardwareMap); ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap); Follower robot = new Follower(hardwareMap); + firstPath.moveToPath1(robot); /* Robot stuff */ robot.setStartingPose(startPose); - path = robot.pathBuilder() .addPath( + path = robot.pathBuilder().addPath( // Line 1 new BezierLine( - new Point(36.000, 72.000, Point.CARTESIAN), - new Point(37.500, 72.000, Point.CARTESIAN) + new Point(37.500, 72.000, Point.CARTESIAN), + new Point(36.000, 72.000, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)).build(); @@ -115,7 +110,7 @@ public class SpecimenTest extends LinearOpMode { wrist.init(); arm.init(); claw.init(); - robot.setMaxPower(.75); + robot.setMaxPower(.40); robot.followPath(path); waitForStart(); @@ -146,26 +141,19 @@ public class SpecimenTest extends LinearOpMode { claw.switchState(); } if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) { + robot.update(); Actions.runBlocking( new SequentialAction( wrist.toFloorPosition(), lift.toHighRung(), wrist.toSpeciemenBar(), lift.dropToHighRung() - + // reverseMoveToPath(robot), + // claw.openClaw(), + // wrist.toFloorPosition(), + // lift.toFloorPosition() ) ); - - } - if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) { - Actions.runBlocking( - new SequentialAction( - claw.openClaw(), - wrist.toFloorPosition(), - lift.toFloorPosition() - ) - ); - } @@ -185,10 +173,12 @@ public class SpecimenTest extends LinearOpMode { lift.setPosition(lift.getPosition() - 25); } - if (currentGamepad2.x && !previousGamepad2.x){ - while(true) { + if (currentGamepad2.x && !previousGamepad2.x) { + while (true) { robot.update(); if (!robot.isBusy()) { + robot.breakFollowing(); + robot.startTeleopDrive(); break; } } @@ -204,4 +194,4 @@ public class SpecimenTest extends LinearOpMode { telemetry.update(); } } -} +} \ No newline at end of file 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 ff0934b..0fa2afc 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 @@ -22,13 +22,13 @@ public class RobotConstants { public final static int liftToFloorPos = 350; public final static int liftToSubmarinePos = 350; - public final static int liftToLowBucketPos = 2250; + public final static int liftToLowBucketPos = 2650; public final static int liftToHighRung = 2100; public final static int dropToHighRung = 1675; public final static int liftToHighRungAttach = 1050; - public final static int liftToHighBucketPos = 3850; - public final static double liftPower = .625; + public final static int liftToHighBucketPos = 4900; + public final static double liftPower = 1; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java index 13d22c3..86652bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java @@ -1,81 +1,36 @@ package org.firstinspires.ftc.teamcode.subsystem; -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; 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; - /* AutoLine# - This file does something of a path...... */ -public class AutoLine1 implements Action { +public class AutoLine1 { - private Follower actionRobot; - private PathChain pathChain; - - private Pose startPose = new Pose(8,65); - - public AutoLine1(Follower robot) { - this.actionRobot = robot; - this.actionRobot.setStartingPose(startPose); + private Pose startPose = new Pose(36, 72); + public void moveToPath1(Follower robot) { + PathChain pathChain; + robot.setStartingPose(startPose); PathBuilder builder = new PathBuilder(); builder .addPath( - // Line 1 - new BezierLine( - new Point(8.000, 65.000, Point.CARTESIAN), - new Point(36.000, 72.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 2 new BezierCurve( new Point(36.000, 72.000, Point.CARTESIAN), - new Point(36.000, 36.000, Point.CARTESIAN), - new Point(48.000, 36.000, Point.CARTESIAN) + new Point(37.500, 72.000, Point.CARTESIAN) ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 3 - new BezierCurve( - new Point(48.000, 36.000, Point.CARTESIAN), - new Point(72.000, 30.000, Point.CARTESIAN), - new Point(57.000, 24.000, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 4 - new BezierLine( - new Point(57.000, 24.000, Point.CARTESIAN), - new Point(10.000, 24.000, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation(); - + ); pathChain = builder.build(); - - this.actionRobot.followPath(this.pathChain); + robot.followPath(pathChain); } - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - this.actionRobot.update(); - return this.actionRobot.isBusy(); - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java new file mode 100644 index 0000000..a2c1c1e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +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 AutoLine2 { + + + private PathChain pathChain; + + private PathChain goToStore; + + private Pose startPose = new Pose(37.5, 72); + + public AutoLine2(Follower robot) { + robot.setStartingPose(startPose); + pathChain = robot.pathBuilder().addPath( + new BezierLine( + new Point(37.500, 72.000, Point.CARTESIAN), + new Point(36.000, 72.000, Point.CARTESIAN) + ) + ).setConstantHeadingInterpolation(Math.toRadians(0)).build(); + robot.followPath(pathChain); + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java index 4041208..0d44c3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java @@ -19,7 +19,7 @@ public class SkyHookSubsystem { private SkyHookState skyHookState; public SkyHookSubsystem(HardwareMap hardwareMap) { - hook = hardwareMap.get(DcMotor.class, LEFT_ENCODER); + hook = hardwareMap.get(DcMotor.class, "skyhook"); } @@ -34,6 +34,7 @@ public class SkyHookSubsystem { this.hook.setDirection(DcMotorSimple.Direction.REVERSE); this.setState(SkyHookState.DOWN); } + public void init() { hook.setPower(0); hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);