diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Circle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Circle.java new file mode 100644 index 0000000..82f51b7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Circle.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@Autonomous(name = "Simple Circle") +public class Circle extends LinearOpMode { + + private static double RADIUS = 20; + private static double DIAMETER = 2*RADIUS; + + public void runOpMode() throws InterruptedException { + Follower follower = Constants.createFollower(hardwareMap); + + Pose midRight = new Pose(0, 0, 0); + Pose topRight = new Pose(0, RADIUS, Math.toRadians(45)); + Pose topMid = new Pose(-RADIUS, RADIUS, Math.toRadians(90)); + Pose topLeft = new Pose(-DIAMETER, RADIUS, Math.toRadians(135)); + Pose midLeft = new Pose(-DIAMETER, 0, Math.toRadians(180)); + Pose bottomLeft = new Pose(-DIAMETER, -RADIUS, Math.toRadians(225)); + Pose bottomMid = new Pose(-RADIUS, -RADIUS, Math.toRadians(270)); + Pose bottomRight = new Pose(0, -RADIUS, Math.toRadians(315)); + + follower.setStartingPose(midRight); + + PathChain circlepath = follower.pathBuilder() + .addPath(new BezierCurve(midRight, topRight, topMid)) + .setLinearHeadingInterpolation(midRight.getHeading(), topMid.getHeading()) + .addPath(new BezierCurve(topMid, topLeft, midLeft)) + .setLinearHeadingInterpolation(topMid.getHeading(), midLeft.getHeading()) + .addPath(new BezierCurve(midLeft, bottomLeft, bottomMid)) + .setLinearHeadingInterpolation(midLeft.getHeading(), bottomMid.getHeading()) + .addPath(new BezierCurve(bottomMid, bottomRight, midRight)) + .setLinearHeadingInterpolation(bottomMid.getHeading(), midRight.getHeading()) + .build(); + + waitForStart(); + + if (opModeIsActive()) { + follower.followPath(circlepath); + } + + while (opModeIsActive() && follower.isBusy()) { + follower.update(); + } + + + + + + + } + +}