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 b15eea2..e03d466 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java @@ -1,4 +1,6 @@ package org.firstinspires.ftc.teamcode; +import static android.os.SystemClock.sleep; + import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -44,7 +46,7 @@ public class BlueBasketAuto extends OpMode { public void init() { follower = new Follower(hardwareMap); - follower.setMaxPower(.375); + follower.setMaxPower(.45); follower.setStartingPose(startPose); @@ -78,14 +80,14 @@ public class BlueBasketAuto extends OpMode { // Line 4 new BezierLine( new Point(28.000, 121.500, Point.CARTESIAN), - new Point(15.500, 130.500, Point.CARTESIAN) + new Point(18.000, 130.179, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 5 new BezierCurve( - new Point(15.500, 130.500, Point.CARTESIAN), + new Point(18.000, 130.179, Point.CARTESIAN), new Point(59.000, 102.500, Point.CARTESIAN), new Point(68.700, 130.500, Point.CARTESIAN) ) @@ -95,35 +97,50 @@ public class BlueBasketAuto extends OpMode { // Line 6 new BezierLine( new Point(68.700, 130.500, Point.CARTESIAN), - new Point(15.750, 130.500, Point.CARTESIAN) + new Point(18.000, 130.339, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 7 new BezierCurve( - new Point(15.750, 130.500, Point.CARTESIAN), - new Point(58.661, 90.643, Point.CARTESIAN), - new Point(68.786, 130.500, Point.CARTESIAN) + new Point(18.000, 130.339, Point.CARTESIAN), + new Point(49.018, 121.179, Point.CARTESIAN), + new Point(63.804, 135.321, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 8 new BezierLine( - new Point(68.786, 130.500, Point.CARTESIAN), - new Point(53.000, 135.321, Point.CARTESIAN) + new Point(63.804, 135.321, Point.CARTESIAN), + new Point(53.036, 135.161, Point.CARTESIAN) ) ) .setConstantHeadingInterpolation(Math.toRadians(0)) .addPath( // Line 9 new BezierLine( - new Point(53.000, 135.321, Point.CARTESIAN), - new Point(18.300, 135.161, Point.CARTESIAN) + new Point(53.036, 135.161, Point.CARTESIAN), + new Point(18.643, 135.000, Point.CARTESIAN) ) ) - .setTangentHeadingInterpolation().build(); + .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) + ) + ) + .addPath( + // Line 9 + new BezierLine( + new Point(18.643, 135.000, Point.CARTESIAN), + new Point(83.250, 95.464, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build(); follower.followPath(path); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());