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

This commit is contained in:
robotics1
2024-12-08 08:45:30 -08:00
4 changed files with 119 additions and 42 deletions

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.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; 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.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; 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 { public class BlueBasketAuto extends OpMode {
private Telemetry telemetryA; private Telemetry telemetryA;
@ -21,13 +22,13 @@ public class BlueBasketAuto extends OpMode {
private PathChain path; private PathChain path;
private final Pose startPose = new Pose(11.25, 95.75); private final Pose startPose = new Pose(8, 65);
@Override @Override
public void init() { public void init() {
follower = new Follower(hardwareMap); follower = new Follower(hardwareMap);
follower.setMaxPower(.45); follower.setMaxPower(.75);
follower.setStartingPose(startPose); follower.setStartingPose(startPose);
@ -35,99 +36,158 @@ public class BlueBasketAuto extends OpMode {
.addPath( .addPath(
// Line 1 // Line 1
new BezierLine( new BezierLine(
new Point(11.250, 95.750, Point.CARTESIAN), new Point(8.000, 65.000, Point.CARTESIAN),
new Point(37.000, 108.000, Point.CARTESIAN) new Point(33.000, 65.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 2 // Line 2
new BezierCurve( new BezierLine(
new Point(37.000, 108.000, Point.CARTESIAN), new Point(33.000, 65.000, Point.CARTESIAN),
new Point(73.286, 111.536, Point.CARTESIAN), new Point(31.000, 65.000, Point.CARTESIAN)
new Point(67.821, 120.536, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 3 // Line 3
new BezierLine( new BezierCurve(
new Point(67.821, 120.536, Point.CARTESIAN), new Point(31.000, 65.000, Point.CARTESIAN),
new Point(28.000, 121.500, 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( .addPath(
// Line 4 // Line 4
new BezierLine( new BezierLine(
new Point(28.000, 121.500, Point.CARTESIAN), new Point(60.000, 34.000, Point.CARTESIAN),
new Point(18.000, 130.179, Point.CARTESIAN) new Point(60.000, 24.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(180))
.addPath( .addPath(
// Line 5 // Line 5
new BezierCurve( new BezierLine(
new Point(18.000, 130.179, Point.CARTESIAN), new Point(60.000, 24.000, Point.CARTESIAN),
new Point(59.000, 102.500, Point.CARTESIAN), new Point(14.500, 24.000, Point.CARTESIAN)
new Point(68.700, 130.500, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(180))
.addPath( .addPath(
// Line 6 // Line 6
new BezierLine( new BezierLine(
new Point(68.700, 130.500, Point.CARTESIAN), new Point(14.500, 24.000, Point.CARTESIAN),
new Point(18.000, 130.339, Point.CARTESIAN) new Point(18.000, 24.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(180))
.addPath( .addPath(
// Line 7 // Line 1
new BezierCurve( new BezierCurve(
new Point(18.000, 130.339, Point.CARTESIAN), new Point(18.000, 24.000, Point.CARTESIAN),
new Point(49.018, 121.179, Point.CARTESIAN), new Point(18.000, 67.000, Point.CARTESIAN),
new Point(63.804, 135.321, Point.CARTESIAN) new Point(31.000, 67.000, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setTangentHeadingInterpolation()
.addPath( .addPath(
// Line 8 // Line 8
new BezierLine( new BezierLine(
new Point(63.804, 135.321, Point.CARTESIAN), new Point(31.000, 67.500, Point.CARTESIAN),
new Point(53.036, 135.161, Point.CARTESIAN) new Point(33.000, 67.500, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 9 // Line 9
new BezierLine( new BezierLine(
new Point(53.036, 135.161, Point.CARTESIAN), new Point(33.000, 67.500, Point.CARTESIAN),
new Point(18.643, 135.000, Point.CARTESIAN) new Point(31.000, 67.500, Point.CARTESIAN)
) )
) )
.setConstantHeadingInterpolation(Math.toRadians(0)) .setConstantHeadingInterpolation(Math.toRadians(0))
.addPath( .addPath(
// Line 10 // Line 10
new BezierLine( new BezierCurve(
new Point(18.643, 135.000, Point.CARTESIAN), new Point(31.000, 67.500, Point.CARTESIAN),
new Point(72.300, 97.400, 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( .addPath(
// Line 11 // Line 11
new BezierLine( new BezierLine(
new Point(18.643, 135.000, Point.CARTESIAN), new Point(60.000, 34.000, Point.CARTESIAN),
new Point(83.250, 95.464, 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); follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update(); telemetryA.update();
} }
@Override @Override
public void loop() { public void loop() {
follower.update(); follower.update();

View File

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

View File

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

View File

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