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

Aditya's sample code - validated to work with 3 specimen plus parking plus 2 penalties

Browse Source
This commit is contained in:
robotics1
2024-10-29 17:06:35 -07:00
parent c207070b1c
commit 657ec8e624
1 changed files with 29 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java
View File

@ -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());