diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java new file mode 100644 index 0000000..1cd08eb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; + +@Autonomous(name = "Specimen Auto", group = "Dev") + +public class SpecimenAuto extends OpMode { + private Follower follower; + private int state; + private ElapsedTime runtime; + + + public void init(){ + state = 0; + follower = new Follower(hardwareMap); + follower.setMaxPower(.90); + runtime = new ElapsedTime(); + + } + + @Override + public void loop() { + switch(state) { + case 0: + //path 1 + break; + case 1: + //specimen drop + break; + case 2: + //path 2 + break; + case 3: + //specimen pick up + break; + case 4: + //path 3 + break; + case 5: + //specimen drop + break; + case 6: + //path 4 + break; + case 7: + //specimen pick up + break; + case 8: + //path 5 + break; + case 9: + //specimen drop + break; + case 10: + //path 6 + break; + case 11: + //specimen pickup + break; + case 12: + //path 7 + break; + case 13: + //specimen drop + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java new file mode 100644 index 0000000..6fc2181 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java @@ -0,0 +1,170 @@ +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 = "Specimen Test", group = "Competition") +public class SpecimenAutoLineTest extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(8, 60); + + @Override + public void init() { + follower = new Follower(hardwareMap); + + follower.setMaxPower(.35); + + follower.setStartingPose(startPose); + + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierLine( + new Point(8.000, 60.000, Point.CARTESIAN), + new Point(39.500, 60.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) + .addPath( + // Line 2 + new BezierLine( + new Point(39.500, 60.000, Point.CARTESIAN), + new Point(37.000, 60.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 3 + new BezierCurve( + new Point(37.000, 60.000, Point.CARTESIAN), + new Point(16.000, 12.000, Point.CARTESIAN), + new Point(80.000, 54.000, Point.CARTESIAN), + new Point(58.000, 23.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 4 + new BezierLine( + new Point(58.000, 23.500, Point.CARTESIAN), + new Point(14.000, 23.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 5 + new BezierLine( + new Point(14.000, 23.500, Point.CARTESIAN), + new Point(58.000, 23.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 6 + new BezierCurve( + new Point(58.000, 23.500, Point.CARTESIAN), + new Point(64.000, 6.000, Point.CARTESIAN), + new Point(14.000, 12.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 7 + new BezierCurve( + new Point(14.000, 12.000, Point.CARTESIAN), + new Point(60.000, 14.000, Point.CARTESIAN), + new Point(60.000, 7.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 8 + new BezierLine( + new Point(60.000, 7.500, Point.CARTESIAN), + new Point(14.000, 7.500, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 9 + new BezierCurve( + new Point(14.000, 7.500, Point.CARTESIAN), + new Point(34.000, 14.250, Point.CARTESIAN), + new Point(19.000, 24.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 10 + new BezierCurve( + new Point(19.000, 24.000, Point.CARTESIAN), + new Point(39.500, 24.000, Point.CARTESIAN), + new Point(19.000, 64.000, Point.CARTESIAN), + new Point(39.500, 64.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 11 + new BezierLine( + new Point(39.500, 64.000, Point.CARTESIAN), + new Point(37.000, 64.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 12 + new BezierCurve( + new Point(37.000, 64.000, Point.CARTESIAN), + new Point(19.000, 64.000, Point.CARTESIAN), + new Point(37.000, 24.000, Point.CARTESIAN), + new Point(19.000, 24.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 13 + new BezierCurve( + new Point(19.000, 24.000, Point.CARTESIAN), + new Point(39.500, 24.000, Point.CARTESIAN), + new Point(19.000, 68.000, Point.CARTESIAN), + new Point(39.500, 68.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)) + .addPath( + // Line 14 + new BezierLine( + new Point(39.500, 68.000, Point.CARTESIAN), + new Point(37.000, 68.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(180)).build(); + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +}