SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed floor position issue

Browse Source
This commit is contained in:
carlos
2025-01-23 16:28:30 -08:00
parent 59f06440d3
commit fed445b171
1 changed files with 5 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java
View File

@ -13,7 +13,7 @@ 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 = "Pre Loaded Blue Basket Auto", group = "Competition")
@Autonomous(name = "Specimen Test", group = "Competition")
public class PreLoadedBlueBasketAuto extends OpMode {
private Telemetry telemetryA;
@ -21,7 +21,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(7.875, 89.357);
private final Pose startPose = new Pose(8, 55);
@Override
public void init() {
@ -35,79 +35,11 @@ public class PreLoadedBlueBasketAuto extends OpMode {
.addPath(
// Line 1
new BezierLine(
new Point(8.036, 89.196, Point.CARTESIAN),
new Point(10.125, 126.804, Point.CARTESIAN)
new Point(8, 55, Point.CARTESIAN),
new Point(24, 21, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierCurve(
new Point(10.125, 126.804, Point.CARTESIAN),
new Point(37.607, 90.000, Point.CARTESIAN),
new Point(62.357, 119.893, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierCurve(
new Point(62.357, 119.893, Point.CARTESIAN),
new Point(33.750, 112.500, Point.CARTESIAN),
new Point(15.107, 130.661, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierCurve(
new Point(15.107, 130.661, Point.CARTESIAN),
new Point(58.821, 103.018, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(15.107, 130.339, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(15.107, 130.339, Point.CARTESIAN),
new Point(59.625, 126.964, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(57.857, 133.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)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierCurve(
new Point(18.964, 134.679, Point.CARTESIAN),
new Point(84.536, 131.786, Point.CARTESIAN),
new Point(80.036, 96.429, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());