Auto code
This commit is contained in:
@ -0,0 +1,56 @@
|
|||||||
|
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.PathChain;
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||||
|
|
||||||
|
@Autonomous(name = "Auto Test", group = "Dev")
|
||||||
|
public class BlueBasketAutoWithDrop1 extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(8, 65);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
.addPath(
|
||||||
|
// Line 1
|
||||||
|
new BezierCurve(
|
||||||
|
new Point(8.000, 89.000, Point.CARTESIAN),
|
||||||
|
new Point(24.000, 96.000, Point.CARTESIAN),
|
||||||
|
new Point(16.000, 128.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
|
||||||
|
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,57 @@
|
|||||||
|
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.PathChain;
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||||
|
|
||||||
|
@Autonomous(name = "Auto Test", group = "Dev")
|
||||||
|
public class BlueBasketAutoWithDrop2 extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(8, 65);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
|
||||||
|
.addPath(
|
||||||
|
// Line 2
|
||||||
|
new BezierCurve(
|
||||||
|
new Point(16.000, 128.000, Point.CARTESIAN),
|
||||||
|
new Point(16.000, 120.000, Point.CARTESIAN),
|
||||||
|
new Point(32.000, 123.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)).build();
|
||||||
|
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,57 @@
|
|||||||
|
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.PathChain;
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||||
|
|
||||||
|
@Autonomous(name = "Auto Test", group = "Dev")
|
||||||
|
public class BlueBasketAutoWithDrop3 extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(8, 65);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 3
|
||||||
|
new BezierCurve(
|
||||||
|
new Point(32.000, 123.000, Point.CARTESIAN),
|
||||||
|
new Point(16.000, 120.000, Point.CARTESIAN),
|
||||||
|
new Point(16.000, 128.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
|
||||||
|
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,57 @@
|
|||||||
|
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 = "Auto Test", group = "Dev")
|
||||||
|
public class BlueBasketAutoWithDrop4 extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(8, 65);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 4
|
||||||
|
new BezierLine(
|
||||||
|
new Point(16.000, 128.000, Point.CARTESIAN),
|
||||||
|
new Point(32.000, 133.500, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)).build();
|
||||||
|
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,58 @@
|
|||||||
|
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 = "Auto Test", group = "Dev")
|
||||||
|
public class BlueBasketAutoWithDrop5 extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(8, 65);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0))
|
||||||
|
.addPath(
|
||||||
|
// Line 5
|
||||||
|
new BezierLine(
|
||||||
|
new Point(32.000, 133.500, Point.CARTESIAN),
|
||||||
|
new Point(16.000, 128.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
|
||||||
|
|
||||||
|
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,56 @@
|
|||||||
|
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.PathChain;
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
|
||||||
|
|
||||||
|
@Autonomous(name = "Auto Test", group = "Dev")
|
||||||
|
public class BlueBasketAutoWithDrop6 extends OpMode {
|
||||||
|
private Telemetry telemetryA;
|
||||||
|
|
||||||
|
private Follower follower;
|
||||||
|
|
||||||
|
private PathChain path;
|
||||||
|
|
||||||
|
private final Pose startPose = new Pose(8, 65);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init() {
|
||||||
|
follower = new Follower(hardwareMap);
|
||||||
|
|
||||||
|
follower.setMaxPower(.75);
|
||||||
|
|
||||||
|
follower.setStartingPose(startPose);
|
||||||
|
|
||||||
|
path = follower.pathBuilder()
|
||||||
|
.addPath(
|
||||||
|
// Line 6
|
||||||
|
new BezierCurve(
|
||||||
|
new Point(16.000, 128.000, Point.CARTESIAN),
|
||||||
|
new Point(85.000, 132.750, Point.CARTESIAN),
|
||||||
|
new Point(84.000, 97.000, Point.CARTESIAN)
|
||||||
|
)
|
||||||
|
)
|
||||||
|
.setTangentHeadingInterpolation.build();
|
||||||
|
|
||||||
|
follower.followPath(path);
|
||||||
|
|
||||||
|
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||||
|
telemetryA.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void loop() {
|
||||||
|
follower.update();
|
||||||
|
follower.telemetryDebug(telemetryA);
|
||||||
|
}
|
||||||
|
}
|
@ -14,6 +14,7 @@ import java.util.ArrayList;
|
|||||||
* @version 1.0, 3/11/2024
|
* @version 1.0, 3/11/2024
|
||||||
*/
|
*/
|
||||||
public class PathBuilder {
|
public class PathBuilder {
|
||||||
|
public PathBuilder setTangentHeadingInterpolation;
|
||||||
private ArrayList<Path> paths = new ArrayList<>();
|
private ArrayList<Path> paths = new ArrayList<>();
|
||||||
|
|
||||||
private ArrayList<PathCallback> callbacks = new ArrayList<>();
|
private ArrayList<PathCallback> callbacks = new ArrayList<>();
|
||||||
|
Reference in New Issue
Block a user