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

Merge remote-tracking branch 'origin/branch-rc-chassis-14493-subsystem-actions' into branch-rc-chassis-14493-subsystem-actions

Browse Source
This commit is contained in:
robotics1
2024-12-08 08:45:30 -08:00
parent c2bf2a7eac 6df3608b97
commit 1b6f8f9b62
4 changed files with 119 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,10 +10,11 @@ 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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Blue Basket Auto", group = "Competition")
@Autonomous(name = "Auto Test", group = "Dev")
public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA;
@ -21,13 +22,13 @@ public class BlueBasketAuto extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75);
private final Pose startPose = new Pose(8, 65);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setMaxPower(.75);
follower.setStartingPose(startPose);
@ -35,99 +36,158 @@ public class BlueBasketAuto extends OpMode {
.addPath(
// Line 1
new BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN),
new Point(37.000, 108.000, Point.CARTESIAN)
new Point(8.000, 65.000, Point.CARTESIAN),
new Point(33.000, 65.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)
new BezierLine(
new Point(33.000, 65.000, Point.CARTESIAN),
new Point(31.000, 65.000, 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)
new BezierCurve(
new Point(31.000, 65.000, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 4
new BezierLine(
new Point(28.000, 121.500, Point.CARTESIAN),
new Point(18.000, 130.179, Point.CARTESIAN)
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 5
new BezierCurve(
new Point(18.000, 130.179, Point.CARTESIAN),
new Point(59.000, 102.500, Point.CARTESIAN),
new Point(68.700, 130.500, Point.CARTESIAN)
new BezierLine(
new Point(60.000, 24.000, Point.CARTESIAN),
new Point(14.500, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 6
new BezierLine(
new Point(68.700, 130.500, Point.CARTESIAN),
new Point(18.000, 130.339, Point.CARTESIAN)
new Point(14.500, 24.000, Point.CARTESIAN),
new Point(18.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 7
// Line 1
new BezierCurve(
new Point(18.000, 130.339, Point.CARTESIAN),
new Point(49.018, 121.179, Point.CARTESIAN),
new Point(63.804, 135.321, Point.CARTESIAN)
new Point(18.000, 24.000, Point.CARTESIAN),
new Point(18.000, 67.000, Point.CARTESIAN),
new Point(31.000, 67.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setTangentHeadingInterpolation()
.addPath(
// Line 8
new BezierLine(
new Point(63.804, 135.321, Point.CARTESIAN),
new Point(53.036, 135.161, Point.CARTESIAN)
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(33.000, 67.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(53.036, 135.161, Point.CARTESIAN),
new Point(18.643, 135.000, Point.CARTESIAN)
new Point(33.000, 67.500, Point.CARTESIAN),
new Point(31.000, 67.500, Point.CARTESIAN)
)
)
.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)
new BezierCurve(
new Point(31.000, 67.500, Point.CARTESIAN),
new Point(26.000, 32.000, Point.CARTESIAN),
new Point(60.000, 34.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 11
new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN),
new Point(83.250, 95.464, Point.CARTESIAN)
new Point(60.000, 34.000, Point.CARTESIAN),
new Point(60.000, 12.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 12
new BezierLine(
new Point(60.000, 12.000, Point.CARTESIAN),
new Point(12.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 13
new BezierLine(
new Point(12.000, 12.000, Point.CARTESIAN),
new Point(20.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 14
new BezierLine(
new Point(20.000, 12.000, Point.CARTESIAN),
new Point(20.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 15
new BezierLine(
new Point(20.000, 24.000, Point.CARTESIAN),
new Point(14.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 16
new BezierCurve(
new Point(14.000, 24.000, Point.CARTESIAN),
new Point(14.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0))
.addPath(
// Line 17
new BezierLine(
new Point(31.000, 70.000, Point.CARTESIAN),
new Point(33.000, 70.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 18
new BezierLine(
new Point(33.000, 70.000, Point.CARTESIAN),
new Point(31.000, 70.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java
View File

@ -21,6 +21,8 @@ public class BlueNonBasketAuto extends OpMode {
private PathChain path;
private PathChain path2;
private final Pose startPose = new Pose(8.000, 55.000);
@Override
@ -40,7 +42,10 @@ public class BlueNonBasketAuto extends OpMode {
new Point(62.357, 33.107, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(0)).build();
follower.followPath(path);
path2 = follower.pathBuilder()
.addPath(
// Line 2
new BezierLine(
@ -141,7 +146,8 @@ public class BlueNonBasketAuto extends OpMode {
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
follower.followPath(path);
follower.followPath(path2);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.cometbots;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
@ -10,6 +11,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
@ -40,6 +42,11 @@ public class CometBotAutoDevelopment {
private Follower follower;
/*
Actions - Path
*/
private AutoLine1 myFirstPath;
public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
this.claw = new ClawActionsSubsystem(hardwareMap);
@ -92,6 +99,10 @@ public class CometBotAutoDevelopment {
this.telemetry.addData("Lift Position", this.lift.getPosition());
}
public Action autoLine1() {return new AutoLine1(follower);}
/*
Controller: 1
Button: A

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristActionsSubsystem.java
View File

@ -47,7 +47,7 @@ public class WristActionsSubsystem {
}
}
public Action toFloorPosition() {
public Action toFloorPosition() {
return new MoveToPosition(wristFloor, WristState.FLOOR);
}
public Action toSpeciemenBar() {