diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java new file mode 100644 index 0000000..d1d11ae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java @@ -0,0 +1,155 @@ +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 = "Blue Non Basket Auto", group = "Competition") +public class BlueNonBasketAuto extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(8.000, 55.000); + + @Override + public void init() { + follower = new Follower(hardwareMap); + + follower.setMaxPower(.6); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierCurve( + new Point(8.000, 55.000, Point.CARTESIAN), + new Point(27.482, 33.750, Point.CARTESIAN), + new Point(62.357, 33.107, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 2 + new BezierLine( + new Point(62.357, 33.107, Point.CARTESIAN), + new Point(62.000, 27.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 3 + new BezierLine( + new Point(62.000, 27.000, Point.CARTESIAN), + new Point(10.000, 27.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 4 + new BezierLine( + new Point(10.000, 27.000, Point.CARTESIAN), + new Point(61.875, 27.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 5 + new BezierLine( + new Point(61.875, 27.000, Point.CARTESIAN), + new Point(61.714, 17.357, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 6 + new BezierLine( + new Point(61.714, 17.357, Point.CARTESIAN), + new Point(14.464, 17.357, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 7 + new BezierLine( + new Point(14.464, 17.357, Point.CARTESIAN), + new Point(61.714, 17.357, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 8 + new BezierLine( + new Point(61.714, 17.357, Point.CARTESIAN), + new Point(61.554, 8.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 9 + new BezierLine( + new Point(61.554, 8.000, Point.CARTESIAN), + new Point(12.536, 8.196, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 10 + new BezierCurve( + new Point(12.536, 8.196, Point.CARTESIAN), + new Point(52.071, 19.929, Point.CARTESIAN), + new Point(50.786, 33.750, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 11 + new BezierCurve( + new Point(50.786, 33.750, Point.CARTESIAN), + new Point(2.571, 39.375, Point.CARTESIAN), + new Point(20.732, 78.911, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 12 + new BezierCurve( + new Point(20.732, 78.911, Point.CARTESIAN), + new Point(24.429, 111.054, Point.CARTESIAN), + new Point(46.929, 121.018, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(0)) + .addPath( + // Line 13 + new BezierCurve( + new Point(46.929, 121.018, Point.CARTESIAN), + new Point(68.143, 116.357, Point.CARTESIAN), + new Point(63.000, 97.714, 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/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index ea95183..b8c2ef2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -26,7 +26,7 @@ public class PedroConstants { /* Centricity : true is robot-centric movement; false if field-centric movement */ - public static final boolean CENTRICITY = false; + public static final boolean CENTRICITY = true; /* Motor Max Power diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java index 9a15b57..4590249 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java @@ -35,7 +35,7 @@ public class PreLoadedBlueBasketAuto extends OpMode { .addPath( // Line 1 new BezierLine( - new Point(8.036, 89.196, Point.CARTESIAN), + new Point(8, 89, Point.CARTESIAN), new Point(10.125, 126.804, Point.CARTESIAN) ) ) @@ -87,15 +87,15 @@ public class PreLoadedBlueBasketAuto extends OpMode { // Line 7 new BezierLine( new Point(59.625, 126.964, Point.CARTESIAN), - new Point(57.857, 133.071, Point.CARTESIAN) + new Point(57.857, 131.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) + new Point(57.857, 131.071, Point.CARTESIAN), + new Point(18.964, 131.679, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) 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 de1ecae..22cb7f7 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 @@ -104,6 +104,8 @@ public class CometBotTeleopCompetition { this.raiseSkyHook(); this.lowerSkyHook(); + Actions.runBlocking(this.lift.toFloorPosition()); + follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY); follower.update(); @@ -244,7 +246,8 @@ public class CometBotTeleopCompetition { if (this.currentGP2.square && !this.previousGP2.square) { Actions.runBlocking(new SequentialAction( this.wrist.toFloorPosition(), - this.arm.toParkPosition() + this.arm.toParkPosition(), + this.lift.toFloorPosition() )); } } @@ -272,37 +275,37 @@ public class CometBotTeleopCompetition { this.wrist.toFloorPosition() ) ); - } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) { + } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) { Actions.runBlocking( new SequentialAction( - this.arm.toFloorPosition(), - this.wrist.toFloorPosition() +// this.lift.toFloorPosition(), + this.arm.toSubmarinePosition(), + this.wrist.toPickupPosition() ) ); - } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) { + } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) { Actions.runBlocking( new SequentialAction( - this.arm.toSubmarinePosition(), + // this.arm.toSubmarinePosition(), this.wrist.toFloorPosition() ) ); } } } + public void raiseSkyHook() { - if (this.currentGP2.dpad_right && !previousGP2.dpad_right) { - hook.raiseHook(); + hook.raiseHook(currentGP2.left_trigger); - } - } + + public void lowerSkyHook() { - if (this.currentGP2.dpad_left && !previousGP2.dpad_left) { - hook.lowerHook(); + hook.lowerHook(currentGP2.right_trigger); - } - } + } + 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 5dcb5d3..d68052e 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 @@ -10,13 +10,14 @@ public class RobotConstants { public final static double armFloor = 0.7; public final static double armSubmarine = 0.55; public final static double armPark = 0.0; - public final static double armBucket = 0.15; + public final static double armBucket = 0.2; - public final static double wristFloor = 0.3; - public final static double wristBucket = 0; + public final static double wristFloor = 0.55; + public final static double wristBucket = 0.25; + public final static double wristPickup = 0.1; - public final static int liftToFloorPos = 0; - public final static int liftToSubmarinePos = 250; + public final static int liftToFloorPos = 350; + public final static int liftToSubmarinePos = 350; public final static int liftToLowBucketPos = 2250; public final static int liftToHighBucketPos = 3850; public final static double liftPower = .625; 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 42e54bc..d3a2d8b 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,11 +68,11 @@ public class ClawActionsSubsystem { } public void init() { - Actions.runBlocking(closeClaw()); + Actions.runBlocking(openClaw()); } public void start() { - Actions.runBlocking(closeClaw()); + Actions.runBlocking(openClaw()); } public double getPosition() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java index f4994cc..9cca186 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftActionsSubsystem.java @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos; import androidx.annotation.NonNull; @@ -52,7 +53,7 @@ public class LiftActionsSubsystem { } public Action toFloorPosition() { - return new MoveToPosition(liftToFloorPos, LiftState.FLOOR); + return new MoveToPosition(liftToSubmarinePos, LiftState.FLOOR); } public Action toLowBucketPosition() { 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 84f2601..4041208 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 @@ -24,13 +24,13 @@ public class SkyHookSubsystem { - public void raiseHook(){ - this.hook.setPower(1.00); + public void raiseHook(double power){ + this.hook.setPower(power); this.hook.setDirection(DcMotorSimple.Direction.FORWARD); this.setState(SkyHookState.UP); } - public void lowerHook(){ - this.hook.setPower(1.00); + public void lowerHook(double power){ + this.hook.setPower(power); this.hook.setDirection(DcMotorSimple.Direction.REVERSE); this.setState(SkyHookState.DOWN); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java index 91f1cf2..231f707 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup; import androidx.annotation.NonNull; @@ -14,7 +15,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx; public class WristActionsSubsystem { public enum WristState { - FLOOR, BUCKET + FLOOR, BUCKET, PICKUP } public ServoImplEx wrist; @@ -50,6 +51,11 @@ public class WristActionsSubsystem { return new MoveToPosition(wristBucket, WristState.BUCKET); } + public Action toPickupPosition() { + return new MoveToPosition(wristPickup, WristState.PICKUP); + } + + public void setState(WristState wristState) { this.state = wristState; }