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