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 af85af8..e65fb51 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java @@ -10,10 +10,11 @@ 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; -@Autonomous(name = "Blue Basket Auto", group = "Competition") +@Autonomous(name = "Auto Test", group = "Dev") public class BlueBasketAuto extends OpMode { private Telemetry telemetryA; @@ -21,13 +22,13 @@ public class BlueBasketAuto extends OpMode { private PathChain path; - private final Pose startPose = new Pose(11.25, 95.75); + private final Pose startPose = new Pose(8, 65); @Override public void init() { follower = new Follower(hardwareMap); - follower.setMaxPower(.45); + follower.setMaxPower(.75); follower.setStartingPose(startPose); @@ -35,99 +36,158 @@ public class BlueBasketAuto extends OpMode { .addPath( // Line 1 new BezierLine( - new Point(11.250, 95.750, Point.CARTESIAN), - new Point(37.000, 108.000, Point.CARTESIAN) + new Point(8.000, 65.000, Point.CARTESIAN), + new Point(33.000, 65.000, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 2 - new BezierCurve( - new Point(37.000, 108.000, Point.CARTESIAN), - new Point(73.286, 111.536, Point.CARTESIAN), - new Point(67.821, 120.536, Point.CARTESIAN) + 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 BezierLine( - new Point(67.821, 120.536, Point.CARTESIAN), - new Point(28.000, 121.500, Point.CARTESIAN) + 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) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) .addPath( // Line 4 new BezierLine( - new Point(28.000, 121.500, Point.CARTESIAN), - new Point(18.000, 130.179, Point.CARTESIAN) + new Point(60.000, 34.000, Point.CARTESIAN), + new Point(60.000, 24.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(180)) .addPath( // Line 5 - new BezierCurve( - new Point(18.000, 130.179, Point.CARTESIAN), - new Point(59.000, 102.500, Point.CARTESIAN), - new Point(68.700, 130.500, Point.CARTESIAN) + new BezierLine( + new Point(60.000, 24.000, Point.CARTESIAN), + new Point(14.500, 24.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(180)) .addPath( // Line 6 new BezierLine( - new Point(68.700, 130.500, Point.CARTESIAN), - new Point(18.000, 130.339, Point.CARTESIAN) + new Point(14.500, 24.000, Point.CARTESIAN), + new Point(18.000, 24.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(180)) .addPath( - // Line 7 + // Line 1 new BezierCurve( - new Point(18.000, 130.339, Point.CARTESIAN), - new Point(49.018, 121.179, Point.CARTESIAN), - new Point(63.804, 135.321, Point.CARTESIAN) + new Point(18.000, 24.000, Point.CARTESIAN), + new Point(18.000, 67.000, Point.CARTESIAN), + new Point(31.000, 67.000, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setTangentHeadingInterpolation() .addPath( // Line 8 new BezierLine( - new Point(63.804, 135.321, Point.CARTESIAN), - new Point(53.036, 135.161, Point.CARTESIAN) + 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(53.036, 135.161, Point.CARTESIAN), - new Point(18.643, 135.000, Point.CARTESIAN) + new Point(33.000, 67.500, Point.CARTESIAN), + new Point(31.000, 67.500, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 10 - new BezierLine( - new Point(18.643, 135.000, Point.CARTESIAN), - new Point(72.300, 97.400, Point.CARTESIAN) + 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(18.643, 135.000, Point.CARTESIAN), - new Point(83.250, 95.464, Point.CARTESIAN) + new Point(60.000, 34.000, Point.CARTESIAN), + new Point(60.000, 12.000, Point.CARTESIAN) ) ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build(); + .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(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java index d1d11ae..daefa07 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java @@ -21,6 +21,8 @@ public class BlueNonBasketAuto extends OpMode { private PathChain path; + private PathChain path2; + private final Pose startPose = new Pose(8.000, 55.000); @Override @@ -40,7 +42,10 @@ public class BlueNonBasketAuto extends OpMode { new Point(62.357, 33.107, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(0)) + .setConstantHeadingInterpolation(Math.toRadians(0)).build(); + follower.followPath(path); + + path2 = follower.pathBuilder() .addPath( // Line 2 new BezierLine( @@ -141,7 +146,8 @@ public class BlueNonBasketAuto extends OpMode { ) ) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build(); - follower.followPath(path); + + follower.followPath(path2); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA.update(); 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 fb51f72..b650d47 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.cometbots; +import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.ftc.Actions; @@ -10,6 +11,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.states.FieldStates; import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.AutoLine1; import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; @@ -40,6 +42,11 @@ public class CometBotAutoDevelopment { private Follower follower; + /* + Actions - Path + */ + private AutoLine1 myFirstPath; + public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55); this.claw = new ClawActionsSubsystem(hardwareMap); @@ -92,6 +99,10 @@ public class CometBotAutoDevelopment { this.telemetry.addData("Lift Position", this.lift.getPosition()); } + public Action autoLine1() {return new AutoLine1(follower);} + + + /* Controller: 1 Button: A 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 3989181..9ae78de 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 @@ -47,7 +47,7 @@ public class WristActionsSubsystem { } } - public Action toFloorPosition() { + public Action toFloorPosition() { return new MoveToPosition(wristFloor, WristState.FLOOR); } public Action toSpeciemenBar() {