Aditya's sample code - validated to work (2 specimen score out of 3)
This commit is contained in:
@ -0,0 +1,145 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
|
import com.acmerobotics.dashboard.FtcDashboard;
|
||||||
|
import com.acmerobotics.dashboard.config.Config;
|
||||||
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||||
|
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
|
||||||
|
* a circle, but some Bezier curves that have control points set essentially in a square. However,
|
||||||
|
* it turns enough to tune your centripetal force correction and some of your heading. Some lag in
|
||||||
|
* heading is to be expected.
|
||||||
|
*
|
||||||
|
* @author Anyi Lin - 10158 Scott's Bots
|
||||||
|
* @author Aaron Yang - 10158 Scott's Bots
|
||||||
|
* @author Harrison Womack - 10158 Scott's Bots
|
||||||
|
* @version 1.0, 3/12/2024
|
||||||
|
*/
|
||||||
|
@Config
|
||||||
|
@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
|
||||||
|
public class BlueBasketAuto extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(11.25, 95.75);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This initializes the Follower and creates the PathChain for the "circle". Additionally, this
|
||||||
|
* initializes the FTC Dashboard telemetry.
|
||||||
|
*/
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.375);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
.addPath(
|
||||||
|
// Line 1
|
||||||
|
new BezierLine(
|
||||||
|
new Point(11.250, 95.750, Point.CARTESIAN),
|
||||||
|
new Point(37.000, 108.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 2
|
||||||
|
new BezierCurve(
|
||||||
|
new Point(37.000, 108.000, Point.CARTESIAN),
|
||||||
|
new Point(73.286, 111.536, Point.CARTESIAN),
|
||||||
|
new Point(67.821, 120.536, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 3
|
||||||
|
new BezierLine(
|
||||||
|
new Point(67.821, 120.536, Point.CARTESIAN),
|
||||||
|
new Point(28.000, 121.500, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 4
|
||||||
|
new BezierLine(
|
||||||
|
new Point(28.000, 121.500, Point.CARTESIAN),
|
||||||
|
new Point(15.500, 130.500, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 5
|
||||||
|
new BezierCurve(
|
||||||
|
new Point(15.500, 130.500, Point.CARTESIAN),
|
||||||
|
new Point(59.000, 102.500, Point.CARTESIAN),
|
||||||
|
new Point(68.700, 130.500, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setConstantHeadingInterpolation(Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 6
|
||||||
|
new BezierLine(
|
||||||
|
new Point(68.700, 130.500, Point.CARTESIAN),
|
||||||
|
new Point(15.750, 130.500, 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)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.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)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.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)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setTangentHeadingInterpolation().build();
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This runs the OpMode, updating the Follower as well as printing out the debug statements to
|
||||||
|
* the Telemetry, as well as the FTC Dashboard.
|
||||||
|
*/
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
if (follower.atParametricEnd()) {
|
||||||
|
follower.followPath(path);
|
||||||
|
}
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
Reference in New Issue
Block a user