From 707d7b06098d45dd1615a1588bdbc3aade45aba2 Mon Sep 17 00:00:00 2001 From: robotics1 Date: Thu, 14 Nov 2024 18:04:24 -0800 Subject: [PATCH] Working SkyHookSubsystem --- .../ftc/teamcode/PreLoadedBlueBasketAuto.java | 122 ++++++++++++++++++ .../cometbots/CometBotTeleopCompetition.java | 22 ++++ .../teamcode/subsystem/SkyHookSubsystem.java | 52 ++++++++ 3 files changed, 196 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java new file mode 100644 index 0000000..9a15b57 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java @@ -0,0 +1,122 @@ +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 = "Pre Loaded Blue Basket Auto", group = "Competition") +public class PreLoadedBlueBasketAuto extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(7.875, 89.357); + + @Override + public void init() { + follower = new Follower(hardwareMap); + + follower.setMaxPower(.45); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierLine( + new Point(8.036, 89.196, Point.CARTESIAN), + new Point(10.125, 126.804, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 2 + new BezierCurve( + new Point(10.125, 126.804, Point.CARTESIAN), + new Point(37.607, 90.000, Point.CARTESIAN), + new Point(62.357, 119.893, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 3 + new BezierCurve( + new Point(62.357, 119.893, Point.CARTESIAN), + new Point(33.750, 112.500, Point.CARTESIAN), + new Point(15.107, 130.661, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 4 + new BezierCurve( + new Point(15.107, 130.661, Point.CARTESIAN), + new Point(58.821, 103.018, Point.CARTESIAN), + new Point(59.625, 126.964, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 5 + new BezierLine( + new Point(59.625, 126.964, Point.CARTESIAN), + new Point(15.107, 130.339, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 6 + new BezierLine( + new Point(15.107, 130.339, Point.CARTESIAN), + new Point(59.625, 126.964, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 7 + new BezierLine( + new Point(59.625, 126.964, Point.CARTESIAN), + new Point(57.857, 133.071, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 8 + new BezierLine( + new Point(57.857, 133.071, Point.CARTESIAN), + new Point(18.964, 134.679, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 9 + new BezierCurve( + new Point(18.964, 134.679, Point.CARTESIAN), + new Point(84.536, 131.786, Point.CARTESIAN), + new Point(80.036, 96.429, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).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/cometbots/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleopCompetition.java index c36d465..de1ecae 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 @@ -16,6 +16,7 @@ import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem; public class CometBotTeleopCompetition { @@ -28,6 +29,7 @@ public class CometBotTeleopCompetition { public ArmActionsSubsystem arm; public WristActionsSubsystem wrist; public LiftActionsSubsystem lift; + public SkyHookSubsystem hook; /* Controllers @@ -61,6 +63,8 @@ public class CometBotTeleopCompetition { this.arm = new ArmActionsSubsystem(hardwareMap); this.wrist = new WristActionsSubsystem(hardwareMap); this.lift = new LiftActionsSubsystem(hardwareMap); + this.hook = new SkyHookSubsystem(hardwareMap); + this.GP1 = gp1; this.GP2 = gp2; this.telemetry = telemetry; @@ -74,6 +78,7 @@ public class CometBotTeleopCompetition { public void init() { this.motors.init(); + this.hook.init(); this.claw.init(); this.arm.init(); this.wrist.init(); @@ -96,6 +101,8 @@ public class CometBotTeleopCompetition { this.clawControl(); this.decreaseMaxPower(); this.increaseMaxPower(); + this.raiseSkyHook(); + this.lowerSkyHook(); follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY); follower.update(); @@ -282,5 +289,20 @@ public class CometBotTeleopCompetition { } } } + public void raiseSkyHook() { + if (this.currentGP2.dpad_right && !previousGP2.dpad_right) { + hook.raiseHook(); + + } + + } + public void lowerSkyHook() { + if (this.currentGP2.dpad_left && !previousGP2.dpad_left) { + hook.lowerHook(); + + + } + + } } 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 new file mode 100644 index 0000000..84f2601 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/SkyHookSubsystem.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; + +import com.acmerobotics.roadrunner.Action; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class SkyHookSubsystem { + + public DcMotor hook; + + public enum SkyHookState { + UP, DOWN, STOP + } + + private SkyHookState skyHookState; + + public SkyHookSubsystem(HardwareMap hardwareMap) { + hook = hardwareMap.get(DcMotor.class, LEFT_ENCODER); + } + + + + public void raiseHook(){ + this.hook.setPower(1.00); + this.hook.setDirection(DcMotorSimple.Direction.FORWARD); + this.setState(SkyHookState.UP); + } + public void lowerHook(){ + this.hook.setPower(1.00); + this.hook.setDirection(DcMotorSimple.Direction.REVERSE); + this.setState(SkyHookState.DOWN); + } + public void init() { + hook.setPower(0); + hook.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + private void setState(SkyHookState liftState) { + this.skyHookState = liftState; + } + + public SkyHookState getState() { + return this.skyHookState; + } + + + +}