From 15312997345376210d3af743ac6dc13d26a552a9 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 19 Oct 2023 22:11:19 -0400 Subject: [PATCH 1/8] Rename autons --- .../com/stuypulse/robot/RobotContainer.java | 42 +-- .../{DoNothingAuton.java => DoNothing.java} | 4 +- .../{MobilityAuton.java => Mobility.java} | 4 +- ...kWire.java => OnePiecePickupDockBump.java} | 4 +- .../robot/commands/auton/ThreePiece.java | 102 +++--- ...PieceWLowBlue.java => ThreePieceBlue.java} | 4 +- ...hreePieceWire.java => ThreePieceBump.java} | 4 +- .../robot/commands/auton/ThreePieceOld.java | 314 ++++++++++++++++++ ...eePieceWLowRed.java => ThreePieceRed.java} | 4 +- .../{TwoPieceWire.java => TwoPieceBump.java} | 4 +- ...eceDockWire.java => TwoPieceDockBump.java} | 4 +- ...LowWireBlue.java => BCThreePieceBlue.java} | 4 +- .../BCThreePieceBumpBlue.java} | 10 +- ...wWireRed.java => BCThreePieceBumpRed.java} | 4 +- ...eBlue.java => BCTwoPieceDockBumpBlue.java} | 4 +- ...ireRed.java => BCTwoPieceDockBumpRed.java} | 4 +- 16 files changed, 411 insertions(+), 105 deletions(-) rename src/main/java/com/stuypulse/robot/commands/auton/{DoNothingAuton.java => DoNothing.java} (85%) rename src/main/java/com/stuypulse/robot/commands/auton/{MobilityAuton.java => Mobility.java} (89%) rename src/main/java/com/stuypulse/robot/commands/auton/{OnePiecePickupDockWire.java => OnePiecePickupDockBump.java} (97%) rename src/main/java/com/stuypulse/robot/commands/auton/{ThreePieceWLowBlue.java => ThreePieceBlue.java} (99%) rename src/main/java/com/stuypulse/robot/commands/auton/{ThreePieceWire.java => ThreePieceBump.java} (99%) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java rename src/main/java/com/stuypulse/robot/commands/auton/{ThreePieceWLowRed.java => ThreePieceRed.java} (99%) rename src/main/java/com/stuypulse/robot/commands/auton/{TwoPieceWire.java => TwoPieceBump.java} (98%) rename src/main/java/com/stuypulse/robot/commands/auton/{TwoPieceDockWire.java => TwoPieceDockBump.java} (97%) rename src/main/java/com/stuypulse/robot/commands/auton/battlecry/{BCThreePieceWLowWireBlue.java => BCThreePieceBlue.java} (99%) rename src/main/java/com/stuypulse/robot/commands/auton/{ThreePieceWLow.java => battlecry/BCThreePieceBumpBlue.java} (95%) rename src/main/java/com/stuypulse/robot/commands/auton/battlecry/{BCThreePieceWLowWireRed.java => BCThreePieceBumpRed.java} (99%) rename src/main/java/com/stuypulse/robot/commands/auton/battlecry/{BCTwoPieceDockWireBlue.java => BCTwoPieceDockBumpBlue.java} (98%) rename src/main/java/com/stuypulse/robot/commands/auton/battlecry/{BCTwoPieceDockWireRed.java => BCTwoPieceDockBumpRed.java} (98%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 4df6373d..789698b0 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,10 +13,10 @@ import com.stuypulse.robot.commands.arm.*; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.auton.*; -import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceWLowWireBlue; -import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceWLowWireRed; -import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockWireBlue; -import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockWireRed; +import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceBumpBlue; +import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceBumpRed; +import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockBumpBlue; +import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockBumpRed; import com.stuypulse.robot.commands.intake.*; import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.manager.*; @@ -227,36 +227,36 @@ private void configureOperatorBindings() { /**************/ public void configureAutons() { - autonChooser.addOption("Do Nothing", new DoNothingAuton()); - // autonChooser.addOption("Mobility", new MobilityAuton()); + autonChooser.addOption("Do Nothing", new DoNothing()); + // autonChooser.addOption("Mobility", new Mobility()); // One Piece - // autonChooser.addOption("One Dock", new OnePieceDock()); - autonChooser.addOption("One Mobility Dock", new OnePieceMobilityDock()); + autonChooser.addOption("One Piece + Dock", new OnePieceDock()); + autonChooser.addOption("One Piece Mobility + Dock", new OnePieceMobilityDock()); // One Piece Dock // autonChooser.addOption("1.5 Piece Dock", new OnePiecePickupDock()); - // autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockWire()); + // autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockBump()); // Two Piece - autonChooser.addOption("Two", new TwoPiece()); - // autonChooser.addOption("Bump Two", new TwoPieceWire()); - autonChooser.addOption("Two Dock Red", new TwoPieceDockRed()); - autonChooser.addOption("Two Dock Blue", new TwoPieceDockBlue()); - autonChooser.addOption("Bumpless Two Dock Blue", new BCTwoPieceDockWireBlue()); - autonChooser.addOption("Bumpless Two Dock Red", new BCTwoPieceDockWireRed()); + autonChooser.addOption("Two Piece", new TwoPiece()); + autonChooser.addOption("Two Piece Bump", new TwoPieceBump()); + autonChooser.addOption("Two Piece Dock Red", new TwoPieceDockRed()); + autonChooser.addOption("Two Piece Dock Blue", new TwoPieceDockBlue()); + // autonChooser.addOption("Two Piece Dock Bump Removed Blue", new BCTwoPieceDockBumpBlue()); + // autonChooser.addOption("Two Piece Dock Bump Removed Red", new BCTwoPieceDockBumpRed()); // Three Piece - autonChooser.addOption("Three", new ThreePieceWLow()); // basically blue - autonChooser.addOption("Three Red", new ThreePieceWLowRed()); - autonChooser.addOption("Three Blue", new ThreePieceWLowBlue()); - autonChooser.setDefaultOption("Bumpless Three Blue", new BCThreePieceWLowWireBlue()); - autonChooser.addOption("Bumpless Three Red", new BCThreePieceWLowWireRed()); - // autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); + autonChooser.setDefaultOption("Three Piece", new ThreePiece()); // basically blue + autonChooser.addOption("Three Piece Red", new ThreePieceRed()); + autonChooser.addOption("Three Piece Blue", new ThreePieceBlue()); + // autonChooser.addOption("Three Piece Bump Removed Blue", new BCThreePieceBumpBlue()); + // autonChooser.addOption("Three Piece Bump Removed Red", new BCThreePieceBumpRed()); + autonChooser.addOption("Three Piece Bump", new ThreePieceBump()); SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/DoNothingAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/DoNothing.java similarity index 85% rename from src/main/java/com/stuypulse/robot/commands/auton/DoNothingAuton.java rename to src/main/java/com/stuypulse/robot/commands/auton/DoNothing.java index bab38be4..269688c0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/DoNothingAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/DoNothing.java @@ -10,9 +10,9 @@ /*- * This auton does nothing... it is used as a placeholder */ -public class DoNothingAuton extends SequentialCommandGroup { +public class DoNothing extends SequentialCommandGroup { - public DoNothingAuton() { + public DoNothing() { addCommands( /** Do a whole lot of nothing */ ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/MobilityAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java similarity index 89% rename from src/main/java/com/stuypulse/robot/commands/auton/MobilityAuton.java rename to src/main/java/com/stuypulse/robot/commands/auton/Mobility.java index 2fd64ef4..2a2dc6e6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/MobilityAuton.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java @@ -12,11 +12,11 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class MobilityAuton extends SequentialCommandGroup { +public class Mobility extends SequentialCommandGroup { private static final PathConstraints CONSTRAINTS = new PathConstraints(2, 2); - public MobilityAuton() { + public Mobility() { addCommands( new SwerveDriveFollowTrajectory( PathPlanner.loadPath("Mobility", CONSTRAINTS) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java rename to src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java index 39bcd10d..37e35ac1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java @@ -24,7 +24,7 @@ import com.pathplanner.lib.PathPlanner; // The best -public class OnePiecePickupDockWire extends DebugSequentialCommandGroup { +public class OnePiecePickupDockBump extends DebugSequentialCommandGroup { private static final double INTAKE_DEACQUIRE_TIME = 0.5; private static final double INTAKE_ACQUIRE_TIME = 0.5; @@ -36,7 +36,7 @@ public class OnePiecePickupDockWire extends DebugSequentialCommandGroup { private static final PathConstraints INTAKE_PIECE = new PathConstraints(3, 2); private static final PathConstraints DOCK = new PathConstraints(1, 2); - public OnePiecePickupDockWire() { + public OnePiecePickupDockBump() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("1.5 Piece + Dock Wire", INTAKE_PIECE, DOCK), "Intake Piece", "Dock" diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java index cc2804dd..ba2b026b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java @@ -22,6 +22,8 @@ import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -30,33 +32,28 @@ public class ThreePiece extends DebugSequentialCommandGroup { - static class ConeAutonReady extends ArmRoutine { - public ConeAutonReady() { - super(Manager.getInstance()::getReadyTrajectory); + static class AutonMidCubeReady extends ArmRoutine { + public AutonMidCubeReady() { + super(() -> Ready.Mid.kAutonCubeBack); } @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); return new ArmTrajectory() - .addState(new ArmState(dest.getShoulderState(), src.getWristState()) - .setShoulderTolerance(20).setWristLimp(true).setWristTolerance(360)) - .addState(new ArmState(dest.getShoulderState(), dest.getWristState()).setWristTolerance(7) - .setShoulderTolerance(15)); + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(7).setShoulderTolerance(20)); } } - static class AutonReady extends ArmRoutine { - public AutonReady() { - super(() -> { - if (Manager.getInstance().getNodeLevel() == NodeLevel.HIGH) { - return Ready.High.kCubeAutonBack; - } else { - return Ready.Mid.kAutonCubeBack; - } - }); + static class AutonHighCubeReady extends ArmRoutine { + public AutonHighCubeReady() { + super(() -> Ready.High.kCubeAutonBack); } - + @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); @@ -65,9 +62,10 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) .setWristTolerance(45)) .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) - .setWristTolerance(7).setShoulderTolerance(25)); + .setWristTolerance(30).setShoulderTolerance(20)); } } + static class ArmIntakeFirst extends ArmRoutine { public ArmIntakeFirst() { super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); @@ -110,11 +108,14 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { -70.82, 10); // 8.37); - double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double intermediateShoulderDegrees = -60; double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); return new ArmTrajectory() - // .addState(src.getShoulderDegrees(), wristSafeAngle) + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setShoulderTolerance(694) + .setWristTolerance(15)) .addState( new ArmState(intermediateShoulderDegrees, wristSafeAngle) @@ -151,7 +152,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public ThreePiece() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("3 Piece W Low", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); @@ -159,27 +160,14 @@ public ThreePiece() { // initial setup addCommands( - new ManagerSetNodeLevel(NodeLevel.MID), + new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), + new ManagerSetNodeLevel(NodeLevel.LOW), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); - // score first piece - addCommands( - new LEDSet(LEDColor.RED), - new ConeAutonReady() - .withTimeout(1.5) - ); - + // score first piece + intake second piece addCommands( - new LEDSet(LEDColor.BLUE), - new IntakeScore(), - new WaitCommand(0.8) - ); - - // intake second piece - addCommands( - new LEDSet(LEDColor.GREEN), new ParallelDeadlineGroup( @@ -192,10 +180,10 @@ public ThreePiece() { .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) - .andThen(new IntakeAcquire()), + .andThen(new IntakeAcquire()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) - new ArmIntakeFirst() - .withTolerance(4, 10) ), new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) @@ -209,6 +197,13 @@ public ThreePiece() { arm.runOnce(() -> arm.setWristVoltage(0)) ); + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -216,14 +211,16 @@ public ThreePiece() { new LEDSet(LEDColor.RED), - new ParallelDeadlineGroup( - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop(), + new ParallelCommandGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new WaitCommand(0.5).andThen(new IntakeStop()), - new AutonReady(), - new WaitCommand(0.5).andThen(new IntakeStop()) + new AutonMidCubeReady() + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) ), new ManagerSetGridNode(1), @@ -233,13 +230,6 @@ public ThreePiece() { new IntakeStop() ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // intake third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -289,7 +279,7 @@ public ThreePiece() { paths.get("Score Third Piece")) .fieldRelative() .withStop() - .alongWith(new AutonReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), + .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), new ManagerSetGridNode(1), // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), @@ -308,4 +298,4 @@ public ThreePiece() { .withStop() ); } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java similarity index 99% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowBlue.java rename to src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java index 964b6a13..dbeb07da 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java @@ -30,7 +30,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class ThreePieceWLowBlue extends DebugSequentialCommandGroup { +public class ThreePieceBlue extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -149,7 +149,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public ThreePieceWLowBlue() { + public ThreePieceBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("3 Piece W Low - Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java similarity index 99% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java rename to src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java index 23689c47..e600f706 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java @@ -29,7 +29,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class ThreePieceWire extends DebugSequentialCommandGroup { +public class ThreePieceBump extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { super(() -> Ready.Mid.kAutonCubeBack); @@ -141,7 +141,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(4, 3.5); private static final PathConstraints SCORE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(3, 2); - public ThreePieceWire() { + public ThreePieceBump() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("3 Piece Wire", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_SECOND_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, SCORE_THIRD_PIECE_CONSTRAINTS), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java new file mode 100644 index 00000000..d9698913 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java @@ -0,0 +1,314 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.auton; + +import com.stuypulse.robot.commands.arm.routines.*; +import com.stuypulse.robot.commands.intake.*; +import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.manager.*; +import com.stuypulse.robot.commands.swerve.*; +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.ArmTrajectories.Ready; +import com.stuypulse.robot.constants.Settings.Arm.Wrist; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.subsystems.Manager.*; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.util.ArmState; +import com.stuypulse.robot.util.ArmTrajectory; +import com.stuypulse.robot.util.DebugSequentialCommandGroup; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; + +/** + * Old three piece -- scores cone high first + */ +public class ThreePieceOld extends DebugSequentialCommandGroup { + + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(Manager.getInstance()::getReadyTrajectory); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + + return new ArmTrajectory() + .addState(new ArmState(dest.getShoulderState(), src.getWristState()) + .setShoulderTolerance(20).setWristLimp(true).setWristTolerance(360)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()).setWristTolerance(7) + .setShoulderTolerance(15)); + } + } + + static class AutonReady extends ArmRoutine { + public AutonReady() { + super(() -> { + if (Manager.getInstance().getNodeLevel() == NodeLevel.HIGH) { + return Ready.High.kCubeAutonBack; + } else { + return Ready.Mid.kAutonCubeBack; + } + }); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(7).setShoulderTolerance(25)); + } + } + static class ArmIntakeFirst extends ArmRoutine { + public ArmIntakeFirst() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 11); + // 8.37); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + // .addState( + // new ArmState(intermediateShoulderDegrees, wristSafeAngle) + // .setShoulderTolerance(15) + // .setWristTolerance(360)) + + // .addState( + // new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + // .setWristTolerance(360)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + static class ArmIntakeSecond extends ArmRoutine { + public ArmIntakeSecond() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 10); + // 8.37); + double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + .addState( + new ArmState(intermediateShoulderDegrees, wristSafeAngle) + .setShoulderTolerance(15) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + .setWristTolerance(20)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + + private static final double INTAKE_DEACQUIRE_TIME = 0.3; + private static final double INTAKE_STOP_WAIT_TIME = 1; + private static final double INTAKE_WAIT_TIME = 1.0; + private static final double ACQUIRE_WAIT_TIME = 0.02; + private static final double WIGGLE_PERIOD = 0.6; + private static final double WIGGLE_VEL_AMPLITUDE = 0.6; + + private static final PathConstraints INTAKE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.5, 2); + private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(2.2, 1.2); + + private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.5, 3.5); + private static final PathConstraints THIRD_SCORE_PIECE_CONSTRAINTS = new PathConstraints(3, 2); + + private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); + + public ThreePieceOld() { + + var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( + PathPlanner.loadPathGroup("3 Piece", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" + ); + + var arm = Arm.getInstance(); + + // initial setup + addCommands( + new ManagerSetNodeLevel(NodeLevel.MID), + new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), + new ManagerSetScoreSide(ScoreSide.BACK) + ); + + // score first piece + addCommands( + new LEDSet(LEDColor.RED), + new ConeAutonReady() + .withTimeout(1.5) + ); + + addCommands( + new LEDSet(LEDColor.BLUE), + new IntakeScore(), + new WaitCommand(0.8) + ); + + // intake second piece + addCommands( + + new LEDSet(LEDColor.GREEN), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) + .robotRelative() + .withStop(), + // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early + + new IntakeScore() + .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) + .andThen(new IntakeStop()) + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) + .andThen(new IntakeAcquire()), + + new ArmIntakeFirst() + .withTolerance(4, 10) + ), + + new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) + .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))), + + // new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) + // .until(Intake.getInstance()::hasGamePiece) + // .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) + // .withTimeout(ACQUIRE_WAIT_TIME), + + arm.runOnce(() -> arm.setWristVoltage(0)) + ); + + // drive to grid and score second piece :: TODO: make custom arm setpoint for this + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new AutonReady(), + new WaitCommand(0.5).andThen(new IntakeStop()) + ), + + new ManagerSetGridNode(1), + // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + + // intake third piece + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + + new LEDSet(LEDColor.GREEN), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory(paths.get("Intake Third Piece")) + .fieldRelative() + .withStop() + .until(Intake.getInstance()::hasGamePiece), + + new WaitCommand(INTAKE_STOP_WAIT_TIME) + .andThen(new IntakeStop()) + .andThen(new WaitCommand(INTAKE_WAIT_TIME)) + .andThen(new IntakeAcquire()), + + new ArmIntakeSecond() + // .setShoulderVelocityTolerance(10) + // .withTolerance(4, 10) + ), + + new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) + .until(Intake.getInstance()::hasGamePiece) + .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) + .withTimeout(ACQUIRE_WAIT_TIME), + + arm.runOnce(() -> arm.setWristVoltage(0)) + ); + + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(5); + arm.setShoulderVelocityFeedbackDebounce(0.2); + }) + ); + + // drive to grid and score third piece + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + new ManagerSetNodeLevel(NodeLevel.HIGH), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + new SwerveDriveFollowTrajectory( + paths.get("Score Third Piece")) + .fieldRelative() + .withStop() + .alongWith(new AutonReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), + + new ManagerSetGridNode(1), + // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + // back away + addCommands( + new LEDSet(LEDColor.RAINBOW), + + new SwerveDriveFollowTrajectory( + paths.get("Back Away")) + .fieldRelative() + .withStop() + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowRed.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java similarity index 99% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowRed.java rename to src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java index 1ea5e058..fb7c92a9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java @@ -30,7 +30,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class ThreePieceWLowRed extends DebugSequentialCommandGroup { +public class ThreePieceRed extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -149,7 +149,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public ThreePieceWLowRed() { + public ThreePieceRed() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("3 Piece W Low - Red", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java similarity index 98% rename from src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java rename to src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java index 8f3f1e4f..256498d6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java @@ -27,7 +27,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class TwoPieceWire extends DebugSequentialCommandGroup { +public class TwoPieceBump extends DebugSequentialCommandGroup { private static class ConeReady extends ArmRoutine { public ConeReady() { @@ -61,7 +61,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); - public TwoPieceWire() { + public TwoPieceBump() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("2 Piece Wire", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS), "Intake Piece", "Score Piece", "Back Away" diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockWire.java rename to src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java index 07381152..65655f95 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java @@ -19,7 +19,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class TwoPieceDockWire extends DebugSequentialCommandGroup { +public class TwoPieceDockBump extends DebugSequentialCommandGroup { private static final double INTAKE_ACQUIRE_TIME = 0.2; private static final double INTAKE_DEACQUIRE_TIME = 1.0; @@ -30,7 +30,7 @@ public class TwoPieceDockWire extends DebugSequentialCommandGroup { private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2); private static final PathConstraints DOCK_CONSTRAINTS = new PathConstraints(1, 0.5); - public TwoPieceDockWire(){ + public TwoPieceDockBump(){ var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("2 Piece + Dock Wire", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), "Intake Piece" , "Score Piece", "Dock" diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java similarity index 99% rename from src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireBlue.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java index 73322a55..12ce3301 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java @@ -30,7 +30,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class BCThreePieceWLowWireBlue extends DebugSequentialCommandGroup { +public class BCThreePieceBlue extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -151,7 +151,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public BCThreePieceWLowWireBlue() { + public BCThreePieceBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("BC 3 Piece W Low Bump Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java similarity index 95% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java index 2a0b0e1e..d7773c60 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java @@ -3,7 +3,7 @@ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ -package com.stuypulse.robot.commands.auton; +package com.stuypulse.robot.commands.auton.battlecry; import com.stuypulse.robot.commands.arm.routines.*; import com.stuypulse.robot.commands.intake.*; @@ -30,7 +30,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class ThreePieceWLow extends DebugSequentialCommandGroup { +public class BCThreePieceBumpBlue extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -61,6 +61,8 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { return new ArmTrajectory() .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(30).setShoulderTolerance(20)) .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) .setWristTolerance(30).setShoulderTolerance(20)); } @@ -149,10 +151,10 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public ThreePieceWLow() { + public BCThreePieceBumpBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece W Low", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("BC 3 Piece W Low Bump Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireRed.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java similarity index 99% rename from src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireRed.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java index 77f22693..13b9b913 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java @@ -30,7 +30,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class BCThreePieceWLowWireRed extends DebugSequentialCommandGroup { +public class BCThreePieceBumpRed extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -151,7 +151,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public BCThreePieceWLowWireRed() { + public BCThreePieceBumpRed() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("BC 3 Piece W Low Bump Red", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockWireBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpBlue.java similarity index 98% rename from src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockWireBlue.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpBlue.java index 29ba3fa4..227f5155 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockWireBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpBlue.java @@ -32,7 +32,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class BCTwoPieceDockWireBlue extends DebugSequentialCommandGroup { +public class BCTwoPieceDockBumpBlue extends DebugSequentialCommandGroup { static class AutonReady extends ArmRoutine { public AutonReady() { super(() -> { @@ -100,7 +100,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.2, 3.25); private static final PathConstraints DOCK_CONSTRAINTS = new PathConstraints(1, 2); - public BCTwoPieceDockWireBlue() { + public BCTwoPieceDockBumpBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("BC 2 Piece Dock Bump Blue", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), "Intake Piece", "Score Piece", "Dock" diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockWireRed.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java similarity index 98% rename from src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockWireRed.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java index dc4cb0f8..512f642a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockWireRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCTwoPieceDockBumpRed.java @@ -32,7 +32,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class BCTwoPieceDockWireRed extends DebugSequentialCommandGroup { +public class BCTwoPieceDockBumpRed extends DebugSequentialCommandGroup { static class AutonReady extends ArmRoutine { public AutonReady() { super(() -> { @@ -100,7 +100,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.2, 3.5); private static final PathConstraints DOCK_CONSTRAINTS = new PathConstraints(1, 2); - public BCTwoPieceDockWireRed() { + public BCTwoPieceDockBumpRed() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( PathPlanner.loadPathGroup("BC 2 Piece Dock Bump Red", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), "Intake Piece", "Score Piece", "Dock" From 3d0bb9dd4407e026bf160c948b8887dac02345d2 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 19 Oct 2023 22:19:36 -0400 Subject: [PATCH 2/8] Rename path files --- ...{1 Piece + Dock.path => 1 Piece Dock.path} | 0 ...y Dock.path => 1 Piece Mobility Dock.path} | 0 ...ock Wire.path => 1.5 Piece Dock Bump.path} | 0 ... Piece + Dock.path => 1.5 Piece Dock.path} | 0 .../{2 Piece Wire.path => 2 Piece Bump.path} | 0 ... Dock Blue.path => 2 Piece Dock Blue.path} | 0 ... Dock Wire.path => 2 Piece Dock Bump.path} | 0 ... + Dock Red.path => 2 Piece Dock Red.path} | 0 ...ce W Low - Blue.path => 3 Piece Blue.path} | 0 .../{3 Piece Wire.path => 3 Piece Bump.path} | 0 .../{3 Piece W Low.path => 3 Piece Old.path} | 86 +++++++++---------- ...iece W Low - Red.path => 3 Piece Red.path} | 0 src/main/deploy/pathplanner/3 Piece.path | 86 +++++++++---------- ...mp Blue.path => BC 3 Piece Bump Blue.path} | 0 ...Bump Red.path => BC 3 Piece Bump Red.path} | 0 15 files changed, 86 insertions(+), 86 deletions(-) rename src/main/deploy/pathplanner/{1 Piece + Dock.path => 1 Piece Dock.path} (100%) rename src/main/deploy/pathplanner/{1 Piece + Mobility Dock.path => 1 Piece Mobility Dock.path} (100%) rename src/main/deploy/pathplanner/{1.5 Piece + Dock Wire.path => 1.5 Piece Dock Bump.path} (100%) rename src/main/deploy/pathplanner/{1.5 Piece + Dock.path => 1.5 Piece Dock.path} (100%) rename src/main/deploy/pathplanner/{2 Piece Wire.path => 2 Piece Bump.path} (100%) rename src/main/deploy/pathplanner/{2 Piece + Dock Blue.path => 2 Piece Dock Blue.path} (100%) rename src/main/deploy/pathplanner/{2 Piece + Dock Wire.path => 2 Piece Dock Bump.path} (100%) rename src/main/deploy/pathplanner/{2 Piece + Dock Red.path => 2 Piece Dock Red.path} (100%) rename src/main/deploy/pathplanner/{3 Piece W Low - Blue.path => 3 Piece Blue.path} (100%) rename src/main/deploy/pathplanner/{3 Piece Wire.path => 3 Piece Bump.path} (100%) rename src/main/deploy/pathplanner/{3 Piece W Low.path => 3 Piece Old.path} (71%) rename src/main/deploy/pathplanner/{3 Piece W Low - Red.path => 3 Piece Red.path} (100%) rename src/main/deploy/pathplanner/{BC 3 Piece W Low Bump Blue.path => BC 3 Piece Bump Blue.path} (100%) rename src/main/deploy/pathplanner/{BC 3 Piece W Low Bump Red.path => BC 3 Piece Bump Red.path} (100%) diff --git a/src/main/deploy/pathplanner/1 Piece + Dock.path b/src/main/deploy/pathplanner/1 Piece Dock.path similarity index 100% rename from src/main/deploy/pathplanner/1 Piece + Dock.path rename to src/main/deploy/pathplanner/1 Piece Dock.path diff --git a/src/main/deploy/pathplanner/1 Piece + Mobility Dock.path b/src/main/deploy/pathplanner/1 Piece Mobility Dock.path similarity index 100% rename from src/main/deploy/pathplanner/1 Piece + Mobility Dock.path rename to src/main/deploy/pathplanner/1 Piece Mobility Dock.path diff --git a/src/main/deploy/pathplanner/1.5 Piece + Dock Wire.path b/src/main/deploy/pathplanner/1.5 Piece Dock Bump.path similarity index 100% rename from src/main/deploy/pathplanner/1.5 Piece + Dock Wire.path rename to src/main/deploy/pathplanner/1.5 Piece Dock Bump.path diff --git a/src/main/deploy/pathplanner/1.5 Piece + Dock.path b/src/main/deploy/pathplanner/1.5 Piece Dock.path similarity index 100% rename from src/main/deploy/pathplanner/1.5 Piece + Dock.path rename to src/main/deploy/pathplanner/1.5 Piece Dock.path diff --git a/src/main/deploy/pathplanner/2 Piece Wire.path b/src/main/deploy/pathplanner/2 Piece Bump.path similarity index 100% rename from src/main/deploy/pathplanner/2 Piece Wire.path rename to src/main/deploy/pathplanner/2 Piece Bump.path diff --git a/src/main/deploy/pathplanner/2 Piece + Dock Blue.path b/src/main/deploy/pathplanner/2 Piece Dock Blue.path similarity index 100% rename from src/main/deploy/pathplanner/2 Piece + Dock Blue.path rename to src/main/deploy/pathplanner/2 Piece Dock Blue.path diff --git a/src/main/deploy/pathplanner/2 Piece + Dock Wire.path b/src/main/deploy/pathplanner/2 Piece Dock Bump.path similarity index 100% rename from src/main/deploy/pathplanner/2 Piece + Dock Wire.path rename to src/main/deploy/pathplanner/2 Piece Dock Bump.path diff --git a/src/main/deploy/pathplanner/2 Piece + Dock Red.path b/src/main/deploy/pathplanner/2 Piece Dock Red.path similarity index 100% rename from src/main/deploy/pathplanner/2 Piece + Dock Red.path rename to src/main/deploy/pathplanner/2 Piece Dock Red.path diff --git a/src/main/deploy/pathplanner/3 Piece W Low - Blue.path b/src/main/deploy/pathplanner/3 Piece Blue.path similarity index 100% rename from src/main/deploy/pathplanner/3 Piece W Low - Blue.path rename to src/main/deploy/pathplanner/3 Piece Blue.path diff --git a/src/main/deploy/pathplanner/3 Piece Wire.path b/src/main/deploy/pathplanner/3 Piece Bump.path similarity index 100% rename from src/main/deploy/pathplanner/3 Piece Wire.path rename to src/main/deploy/pathplanner/3 Piece Bump.path diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece Old.path similarity index 71% rename from src/main/deploy/pathplanner/3 Piece W Low.path rename to src/main/deploy/pathplanner/3 Piece Old.path index 6f4c39d8..e07b8e3e 100644 --- a/src/main/deploy/pathplanner/3 Piece W Low.path +++ b/src/main/deploy/pathplanner/3 Piece Old.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.8, - "y": 4.6 + "x": 2.19, + "y": 4.84018306114904 }, "prevControl": null, "nextControl": { - "x": 3.1835287054512387, - "y": 4.6 + "x": 3.5735287054512384, + "y": 4.84018306114904 }, "holonomicAngle": 0, "isReversal": false, @@ -24,16 +24,16 @@ }, { "anchorPoint": { - "x": 6.340000000000001, - "y": 4.44 + "x": 6.187741628152297, + "y": 4.440796639600396 }, "prevControl": { - "x": 5.587270042913402, - "y": 4.44 + "x": 5.4365147876203235, + "y": 4.488342642165712 }, "nextControl": { - "x": 6.549202411287385, - "y": 4.44 + "x": 6.396526285895116, + "y": 4.427582420755914 }, "holonomicAngle": 0, "isReversal": false, @@ -49,16 +49,16 @@ }, { "anchorPoint": { - "x": 6.7700000000000005, - "y": 4.44 + "x": 6.62, + "y": 4.4 }, "prevControl": { - "x": 6.651547629778109, - "y": 4.471296466187375 + "x": 6.501545245083374, + "y": 4.431287439087334 }, "nextControl": { - "x": 6.651547629778109, - "y": 4.471296466187375 + "x": 6.501545245083374, + "y": 4.431287439087334 }, "holonomicAngle": 0, "isReversal": true, @@ -74,18 +74,18 @@ }, { "anchorPoint": { - "x": 2.1599999999999997, - "y": 4.4 + "x": 2.26, + "y": 4.449999999999999 }, "prevControl": { - "x": 4.086804808642588, - "y": 4.568573577583465 + "x": 3.5298747164225914, + "y": 4.449999999999999 }, "nextControl": { - "x": 4.086804808642588, - "y": 4.568573577583465 + "x": 3.5298747164225914, + "y": 4.449999999999999 }, - "holonomicAngle": 6.54, + "holonomicAngle": 6.538707501667168, "isReversal": true, "velOverride": null, "isLocked": false, @@ -103,12 +103,12 @@ "y": 3.53 }, "prevControl": { - "x": 5.370212645435206, - "y": 4.73978735456479 + "x": 4.846944355810422, + "y": 4.792637058583726 }, "nextControl": { - "x": 6.712618407320928, - "y": 3.3973815926790722 + "x": 6.731586100906257, + "y": 3.419560252025286 }, "holonomicAngle": -30.0, "isReversal": false, @@ -128,12 +128,12 @@ "y": 3.25 }, "prevControl": { - "x": 6.842860523327884, - "y": 3.3115587598696563 + "x": 6.894183252779317, + "y": 3.29693179137248 }, "nextControl": { - "x": 6.842860523327884, - "y": 3.3115587598696563 + "x": 6.894183252779317, + "y": 3.29693179137248 }, "holonomicAngle": -30.0, "isReversal": true, @@ -149,16 +149,16 @@ }, { "anchorPoint": { - "x": 1.7, - "y": 4.4 + "x": 1.9, + "y": 4.35 }, "prevControl": { - "x": 5.506269559767853, - "y": 5.278746568199897 + "x": 6.043518753704175, + "y": 5.14849401579145 }, "nextControl": { - "x": 5.506269559767853, - "y": 5.278746568199897 + "x": 6.043518753704175, + "y": 5.14849401579145 }, "holonomicAngle": 10.0, "isReversal": true, @@ -174,12 +174,12 @@ }, { "anchorPoint": { - "x": 7.60794478853915, - "y": 4.990176476079449 + "x": 5.4365147876203235, + "y": 4.754600256531473 }, "prevControl": { - "x": 6.851165372882417, - "y": 4.737916670860539 + "x": 4.770870751705916, + "y": 4.754600256531473 }, "nextControl": null, "holonomicAngle": 0, @@ -195,8 +195,8 @@ } } ], - "maxVelocity": 6940000.0, - "maxAcceleration": 6940000.0, + "maxVelocity": 4.0, + "maxAcceleration": 3.0, "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/3 Piece W Low - Red.path b/src/main/deploy/pathplanner/3 Piece Red.path similarity index 100% rename from src/main/deploy/pathplanner/3 Piece W Low - Red.path rename to src/main/deploy/pathplanner/3 Piece Red.path diff --git a/src/main/deploy/pathplanner/3 Piece.path b/src/main/deploy/pathplanner/3 Piece.path index e07b8e3e..6f4c39d8 100644 --- a/src/main/deploy/pathplanner/3 Piece.path +++ b/src/main/deploy/pathplanner/3 Piece.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 2.19, - "y": 4.84018306114904 + "x": 1.8, + "y": 4.6 }, "prevControl": null, "nextControl": { - "x": 3.5735287054512384, - "y": 4.84018306114904 + "x": 3.1835287054512387, + "y": 4.6 }, "holonomicAngle": 0, "isReversal": false, @@ -24,16 +24,16 @@ }, { "anchorPoint": { - "x": 6.187741628152297, - "y": 4.440796639600396 + "x": 6.340000000000001, + "y": 4.44 }, "prevControl": { - "x": 5.4365147876203235, - "y": 4.488342642165712 + "x": 5.587270042913402, + "y": 4.44 }, "nextControl": { - "x": 6.396526285895116, - "y": 4.427582420755914 + "x": 6.549202411287385, + "y": 4.44 }, "holonomicAngle": 0, "isReversal": false, @@ -49,16 +49,16 @@ }, { "anchorPoint": { - "x": 6.62, - "y": 4.4 + "x": 6.7700000000000005, + "y": 4.44 }, "prevControl": { - "x": 6.501545245083374, - "y": 4.431287439087334 + "x": 6.651547629778109, + "y": 4.471296466187375 }, "nextControl": { - "x": 6.501545245083374, - "y": 4.431287439087334 + "x": 6.651547629778109, + "y": 4.471296466187375 }, "holonomicAngle": 0, "isReversal": true, @@ -74,18 +74,18 @@ }, { "anchorPoint": { - "x": 2.26, - "y": 4.449999999999999 + "x": 2.1599999999999997, + "y": 4.4 }, "prevControl": { - "x": 3.5298747164225914, - "y": 4.449999999999999 + "x": 4.086804808642588, + "y": 4.568573577583465 }, "nextControl": { - "x": 3.5298747164225914, - "y": 4.449999999999999 + "x": 4.086804808642588, + "y": 4.568573577583465 }, - "holonomicAngle": 6.538707501667168, + "holonomicAngle": 6.54, "isReversal": true, "velOverride": null, "isLocked": false, @@ -103,12 +103,12 @@ "y": 3.53 }, "prevControl": { - "x": 4.846944355810422, - "y": 4.792637058583726 + "x": 5.370212645435206, + "y": 4.73978735456479 }, "nextControl": { - "x": 6.731586100906257, - "y": 3.419560252025286 + "x": 6.712618407320928, + "y": 3.3973815926790722 }, "holonomicAngle": -30.0, "isReversal": false, @@ -128,12 +128,12 @@ "y": 3.25 }, "prevControl": { - "x": 6.894183252779317, - "y": 3.29693179137248 + "x": 6.842860523327884, + "y": 3.3115587598696563 }, "nextControl": { - "x": 6.894183252779317, - "y": 3.29693179137248 + "x": 6.842860523327884, + "y": 3.3115587598696563 }, "holonomicAngle": -30.0, "isReversal": true, @@ -149,16 +149,16 @@ }, { "anchorPoint": { - "x": 1.9, - "y": 4.35 + "x": 1.7, + "y": 4.4 }, "prevControl": { - "x": 6.043518753704175, - "y": 5.14849401579145 + "x": 5.506269559767853, + "y": 5.278746568199897 }, "nextControl": { - "x": 6.043518753704175, - "y": 5.14849401579145 + "x": 5.506269559767853, + "y": 5.278746568199897 }, "holonomicAngle": 10.0, "isReversal": true, @@ -174,12 +174,12 @@ }, { "anchorPoint": { - "x": 5.4365147876203235, - "y": 4.754600256531473 + "x": 7.60794478853915, + "y": 4.990176476079449 }, "prevControl": { - "x": 4.770870751705916, - "y": 4.754600256531473 + "x": 6.851165372882417, + "y": 4.737916670860539 }, "nextControl": null, "holonomicAngle": 0, @@ -195,8 +195,8 @@ } } ], - "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxVelocity": 6940000.0, + "maxAcceleration": 6940000.0, "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/BC 3 Piece W Low Bump Blue.path b/src/main/deploy/pathplanner/BC 3 Piece Bump Blue.path similarity index 100% rename from src/main/deploy/pathplanner/BC 3 Piece W Low Bump Blue.path rename to src/main/deploy/pathplanner/BC 3 Piece Bump Blue.path diff --git a/src/main/deploy/pathplanner/BC 3 Piece W Low Bump Red.path b/src/main/deploy/pathplanner/BC 3 Piece Bump Red.path similarity index 100% rename from src/main/deploy/pathplanner/BC 3 Piece W Low Bump Red.path rename to src/main/deploy/pathplanner/BC 3 Piece Bump Red.path From 7c4b1f501ed17a1a0e4c395c675a7f44a5871f3f Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 19 Oct 2023 22:21:51 -0400 Subject: [PATCH 3/8] Remove duplicate auto --- .../auton/battlecry/BCThreePieceBlue.java | 303 ------------------ 1 file changed, 303 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java deleted file mode 100644 index 12ce3301..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBlue.java +++ /dev/null @@ -1,303 +0,0 @@ -/************************ PROJECT JIM *************************/ -/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ -/* This work is licensed under the terms of the MIT license. */ -/**************************************************************/ - -package com.stuypulse.robot.commands.auton.battlecry; - -import com.stuypulse.robot.commands.arm.routines.*; -import com.stuypulse.robot.commands.intake.*; -import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.manager.*; -import com.stuypulse.robot.commands.swerve.*; -import com.stuypulse.robot.constants.ArmTrajectories; -import com.stuypulse.robot.constants.ArmTrajectories.Ready; -import com.stuypulse.robot.constants.Settings.Arm.Wrist; -import com.stuypulse.robot.subsystems.Manager; -import com.stuypulse.robot.subsystems.Manager.*; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.util.ArmState; -import com.stuypulse.robot.util.ArmTrajectory; -import com.stuypulse.robot.util.DebugSequentialCommandGroup; -import com.stuypulse.robot.util.LEDColor; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; - -public class BCThreePieceBlue extends DebugSequentialCommandGroup { - - static class AutonMidCubeReady extends ArmRoutine { - public AutonMidCubeReady() { - super(() -> Ready.Mid.kAutonCubeBack); - } - - @Override - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); - - return new ArmTrajectory() - .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) - .setWristTolerance(45)) - .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) - .setWristTolerance(7).setShoulderTolerance(20)); - } - } - - static class AutonHighCubeReady extends ArmRoutine { - public AutonHighCubeReady() { - super(() -> Ready.High.kCubeAutonBack); - } - - @Override - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); - - return new ArmTrajectory() - .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) - .setWristTolerance(45)) - .addState(new ArmState(dest.getShoulderDegrees(), wristSafeAngle) - .setWristTolerance(30).setShoulderTolerance(20)) - .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) - .setWristTolerance(30).setShoulderTolerance(20)); - } - } - - static class ArmIntakeFirst extends ArmRoutine { - public ArmIntakeFirst() { - super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); - } - - @Override - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - dest = new ArmState( - -70.82, - 11); - // 8.37); - - return new ArmTrajectory() - // .addState(src.getShoulderDegrees(), wristSafeAngle) - - // .addState( - // new ArmState(intermediateShoulderDegrees, wristSafeAngle) - // .setShoulderTolerance(15) - // .setWristTolerance(360)) - - // .addState( - // new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) - // .setWristTolerance(360)) - - .addState( - new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) - .setShoulderTolerance(3) - .setWristTolerance(4)); - } - } - - static class ArmIntakeSecond extends ArmRoutine { - public ArmIntakeSecond() { - super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); - } - - @Override - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - dest = new ArmState( - -70.82, - 10); - // 8.37); - // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); - double intermediateShoulderDegrees = -60; - double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); - - return new ArmTrajectory() - .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) - .setShoulderTolerance(694) - .setWristTolerance(15)) - - .addState( - new ArmState(intermediateShoulderDegrees, wristSafeAngle) - .setShoulderTolerance(15) - .setWristTolerance(15)) - - .addState( - new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) - .setWristTolerance(20)) - - .addState( - new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) - .setShoulderTolerance(3) - .setWristTolerance(4)); - } - } - - - private static final double INTAKE_DEACQUIRE_TIME = 0.3; - private static final double INTAKE_STOP_WAIT_TIME = 1; - private static final double INTAKE_WAIT_TIME = 1.0; - private static final double ACQUIRE_WAIT_TIME = 0.02; - private static final double WIGGLE_PERIOD = 0.6; - private static final double WIGGLE_VEL_AMPLITUDE = 0.6; - - private static final PathConstraints INTAKE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.5, 2); - private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(2.2, 1.2); - - private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.5, 3.5); - private static final PathConstraints THIRD_SCORE_PIECE_CONSTRAINTS = new PathConstraints(3, 2); - - private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - - public BCThreePieceBlue() { - - var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("BC 3 Piece W Low Bump Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), - "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" - ); - - var arm = Arm.getInstance(); - - // initial setup - addCommands( - new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), - new ManagerSetNodeLevel(NodeLevel.LOW), - new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), - new ManagerSetScoreSide(ScoreSide.BACK) - ); - - // score first piece + intake second piece - addCommands( - new LEDSet(LEDColor.GREEN), - - new ParallelDeadlineGroup( - new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) - .robotRelative() - .withStop(), - // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early - - new IntakeScore() - .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) - .andThen(new IntakeStop()) - .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) - .andThen(new IntakeAcquire()) - .andThen(new ArmIntakeFirst() - .withTolerance(4, 10)) - - ), - - new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) - .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))), - - // new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) - // .until(Intake.getInstance()::hasGamePiece) - // .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) - // .withTimeout(ACQUIRE_WAIT_TIME), - - arm.runOnce(() -> arm.setWristVoltage(0)) - ); - - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - - // drive to grid and score second piece :: TODO: make custom arm setpoint for this - addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), - new ManagerSetScoreSide(ScoreSide.BACK), - - new LEDSet(LEDColor.RED), - - new ParallelCommandGroup( - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop(), - - new WaitCommand(0.5).andThen(new IntakeStop()), - - new AutonMidCubeReady() - .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) - ), - - new ManagerSetGridNode(1), - // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), - new IntakeDeacquire(), - new WaitCommand(INTAKE_DEACQUIRE_TIME), - new IntakeStop() - ); - - // intake third piece - addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), - - new LEDSet(LEDColor.GREEN), - - new ParallelDeadlineGroup( - new SwerveDriveFollowTrajectory(paths.get("Intake Third Piece")) - .fieldRelative() - .withStop() - .until(Intake.getInstance()::hasGamePiece), - - new WaitCommand(INTAKE_STOP_WAIT_TIME) - .andThen(new IntakeStop()) - .andThen(new WaitCommand(INTAKE_WAIT_TIME)) - .andThen(new IntakeAcquire()), - - new ArmIntakeSecond() - // .setShoulderVelocityTolerance(10) - // .withTolerance(4, 10) - ), - - new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) - .until(Intake.getInstance()::hasGamePiece) - .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) - .withTimeout(ACQUIRE_WAIT_TIME), - - arm.runOnce(() -> arm.setWristVoltage(0)) - ); - - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(5); - arm.setShoulderVelocityFeedbackDebounce(0.2); - }) - ); - - // drive to grid and score third piece - addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), - new ManagerSetNodeLevel(NodeLevel.HIGH), - new ManagerSetScoreSide(ScoreSide.BACK), - - new LEDSet(LEDColor.RED), - - new SwerveDriveFollowTrajectory( - paths.get("Score Third Piece")) - .fieldRelative() - .withStop() - .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), - - new ManagerSetGridNode(1), - // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), - new IntakeDeacquire(), - new WaitCommand(INTAKE_DEACQUIRE_TIME), - new IntakeStop() - ); - - // back away - addCommands( - new LEDSet(LEDColor.RAINBOW), - - new SwerveDriveFollowTrajectory( - paths.get("Back Away")) - .fieldRelative() - .withStop() - ); - } -} \ No newline at end of file From 2e54a0152e8891e55f5fa3e9dea2b5d7bbb7187f Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 19 Oct 2023 22:27:34 -0400 Subject: [PATCH 4/8] Rename paths in auton loading --- .../stuypulse/robot/commands/auton/OnePieceDock.java | 2 +- .../robot/commands/auton/OnePieceMobilityDock.java | 11 ++--------- .../robot/commands/auton/OnePiecePickupDock.java | 2 +- .../robot/commands/auton/OnePiecePickupDockBump.java | 2 +- .../stuypulse/robot/commands/auton/ThreePiece.java | 2 +- .../robot/commands/auton/ThreePieceBlue.java | 2 +- .../robot/commands/auton/ThreePieceBump.java | 2 +- .../stuypulse/robot/commands/auton/ThreePieceOld.java | 2 +- .../stuypulse/robot/commands/auton/ThreePieceRed.java | 2 +- .../stuypulse/robot/commands/auton/TwoPieceBump.java | 2 +- .../robot/commands/auton/TwoPieceDockBlue.java | 2 +- .../robot/commands/auton/TwoPieceDockBump.java | 2 +- .../robot/commands/auton/TwoPieceDockRed.java | 2 +- .../auton/battlecry/BCThreePieceBumpBlue.java | 2 +- .../commands/auton/battlecry/BCThreePieceBumpRed.java | 2 +- 15 files changed, 16 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java index 97ea16be..fda7ae96 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java @@ -74,7 +74,7 @@ public OnePieceDock() { addCommands( new LEDSet(LEDColor.PURPLE), new ParallelDeadlineGroup( - new SwerveDriveFollowTrajectory(PathPlanner.loadPath("1 Piece + Dock", DOCK)) + new SwerveDriveFollowTrajectory(PathPlanner.loadPath("1 Piece Dock", DOCK)) .robotRelative().withStop(), new WaitCommand(INTAKE_ACQUIRE_TIME).andThen(new IntakeStop()).andThen(new ArmStow()) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java index 383681d7..d95a6579 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -39,16 +39,12 @@ public ConeAutonReady() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - // return new ArmTrajectory() - // .addState(new ArmState(dest.getShoulderState(), src.getWristState()) - // .setShoulderTolerance(20).setWristTolerance(360)) - // .addState(new) return new ArmTrajectory() .addState( new ArmState(dest.getShoulderDegrees(), src.getWristDegrees()) .setWristLimp(true)) .addState(dest); - } + } } private class FastStow extends ArmRoutine { @@ -74,7 +70,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public OnePieceMobilityDock() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("1 Piece + Mobility Dock", OVER_CHARGE, DOCK), + PathPlanner.loadPathGroup("1 Piece Mobility Dock", OVER_CHARGE, DOCK), "Over Charge", "Dock" ); @@ -89,9 +85,6 @@ public OnePieceMobilityDock() { addCommands( new LEDSet(LEDColor.RED), new ConeAutonReady() - // .setWristVelocityTolerance(25) - // .setShoulderVelocityTolerance(45) - // .withTolerance(7, 9) .withTimeout(3) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java index 98f4c997..a411639e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java @@ -37,7 +37,7 @@ public class OnePiecePickupDock extends DebugSequentialCommandGroup { public OnePiecePickupDock() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("1.5 Piece + Dock", INTAKE_PIECE, DOCK), + PathPlanner.loadPathGroup("1.5 Piece Dock", INTAKE_PIECE, DOCK), "Intake Piece", "Dock" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java index 37e35ac1..ed3bcad0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java @@ -38,7 +38,7 @@ public class OnePiecePickupDockBump extends DebugSequentialCommandGroup { public OnePiecePickupDockBump() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("1.5 Piece + Dock Wire", INTAKE_PIECE, DOCK), + PathPlanner.loadPathGroup("1.5 Piece Dock Bump", INTAKE_PIECE, DOCK), "Intake Piece", "Dock" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java index ba2b026b..c33fcb7a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java @@ -152,7 +152,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public ThreePiece() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece W Low", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("3 Piece", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java index dbeb07da..8a7f59bb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java @@ -152,7 +152,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public ThreePieceBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece W Low - Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("3 Piece Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java index e600f706..4cddf7e4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java @@ -144,7 +144,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public ThreePieceBump() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece Wire", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_SECOND_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, SCORE_THIRD_PIECE_CONSTRAINTS), + PathPlanner.loadPathGroup("3 Piece Bump", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_SECOND_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, SCORE_THIRD_PIECE_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java index d9698913..45b0ddd5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java @@ -154,7 +154,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public ThreePieceOld() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("3 Piece Old", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java index fb7c92a9..5a875860 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java @@ -152,7 +152,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public ThreePieceRed() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece W Low - Red", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("3 Piece Red", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java index 256498d6..45ef7b21 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java @@ -63,7 +63,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public TwoPieceBump() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("2 Piece Wire", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS), + PathPlanner.loadPathGroup("2 Piece Bump", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS), "Intake Piece", "Score Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java index 2656a490..36843be8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java @@ -102,7 +102,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public TwoPieceDockBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("2 Piece + Dock Blue", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), + PathPlanner.loadPathGroup("2 Piece Dock Blue", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), "Intake Piece", "Score Piece", "Dock" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java index 65655f95..ae15a467 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java @@ -32,7 +32,7 @@ public class TwoPieceDockBump extends DebugSequentialCommandGroup { public TwoPieceDockBump(){ var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("2 Piece + Dock Wire", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), + PathPlanner.loadPathGroup("2 Piece Dock Bump", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), "Intake Piece" , "Score Piece", "Dock" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java index ceccb1bc..a634f668 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java @@ -102,7 +102,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public TwoPieceDockRed() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("2 Piece + Dock Red", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), + PathPlanner.loadPathGroup("2 Piece Dock Red", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, DOCK_CONSTRAINTS), "Intake Piece", "Score Piece", "Dock" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java index d7773c60..40e178bc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java @@ -154,7 +154,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public BCThreePieceBumpBlue() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("BC 3 Piece W Low Bump Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("BC 3 Piece Bump Blue", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java index 13b9b913..8db38e8a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java @@ -154,7 +154,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { public BCThreePieceBumpRed() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("BC 3 Piece W Low Bump Red", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + PathPlanner.loadPathGroup("BC 3 Piece Bump Red", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); From 2fb03e407439c13d75f77c3d9e19cdf912fd408d Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 19 Oct 2023 22:30:07 -0400 Subject: [PATCH 5/8] Modify first score of 2 Bump, 2 Dock to mid, shift start of paths --- src/main/deploy/pathplanner/2 Piece Bump.path | 4 +-- .../deploy/pathplanner/2 Piece Dock Blue.path | 4 +-- .../deploy/pathplanner/2 Piece Dock Red.path | 4 +-- .../robot/commands/auton/TwoPieceBump.java | 28 +++++++---------- .../commands/auton/TwoPieceDockBlue.java | 31 +++++++++---------- .../robot/commands/auton/TwoPieceDockRed.java | 30 +++++++++--------- 6 files changed, 47 insertions(+), 54 deletions(-) diff --git a/src/main/deploy/pathplanner/2 Piece Bump.path b/src/main/deploy/pathplanner/2 Piece Bump.path index 365b0178..f73ad063 100644 --- a/src/main/deploy/pathplanner/2 Piece Bump.path +++ b/src/main/deploy/pathplanner/2 Piece Bump.path @@ -2,12 +2,12 @@ "waypoints": [ { "anchorPoint": { - "x": 1.9941842018915368, + "x": 2.144, "y": 0.5 }, "prevControl": null, "nextControl": { - "x": 2.994184201891537, + "x": 3.144, "y": 0.5 }, "holonomicAngle": 0, diff --git a/src/main/deploy/pathplanner/2 Piece Dock Blue.path b/src/main/deploy/pathplanner/2 Piece Dock Blue.path index 1a21a46e..54a5123f 100644 --- a/src/main/deploy/pathplanner/2 Piece Dock Blue.path +++ b/src/main/deploy/pathplanner/2 Piece Dock Blue.path @@ -2,12 +2,12 @@ "waypoints": [ { "anchorPoint": { - "x": 1.9941842018915368, + "x": 2.144, "y": 4.84018306114904 }, "prevControl": null, "nextControl": { - "x": 2.994184201891537, + "x": 3.144, "y": 4.84018306114904 }, "holonomicAngle": 0, diff --git a/src/main/deploy/pathplanner/2 Piece Dock Red.path b/src/main/deploy/pathplanner/2 Piece Dock Red.path index 031855cf..52e95cbc 100644 --- a/src/main/deploy/pathplanner/2 Piece Dock Red.path +++ b/src/main/deploy/pathplanner/2 Piece Dock Red.path @@ -2,12 +2,12 @@ "waypoints": [ { "anchorPoint": { - "x": 1.9941842018915368, + "x": 2.144, "y": 4.84018306114904 }, "prevControl": null, "nextControl": { - "x": 2.994184201891537, + "x": 3.144, "y": 4.84018306114904 }, "holonomicAngle": 0, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java index 45ef7b21..b79adfc5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java @@ -29,25 +29,24 @@ public class TwoPieceBump extends DebugSequentialCommandGroup { - private static class ConeReady extends ArmRoutine { - public ConeReady() { - super(Manager.getInstance()::getReadyTrajectory); + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(() -> { + if (Manager.getInstance().getNodeLevel() == NodeLevel.HIGH) + return new ArmState(-179, 136 - 8); + else + return new ArmState(-161.7, 133.9); + }); } - @Override + @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { return new ArmTrajectory() - .addState( new ArmState(dest.getShoulderDegrees(), src.getWristDegrees()) .setWristLimp(true)) - - // .addState( - // dest.getShoulderDegrees(), - // (Manager.getInstance().getNodeLevel() == NodeLevel.MID) ? dest.getWristDegrees() : (src.getWristDegrees() + dest.getWristDegrees()) / 2.0) - .addState(dest); - } + } } private static final double INTAKE_DEACQUIRE_TIME = 0.5; @@ -80,11 +79,8 @@ public TwoPieceBump() { // score first piece addCommands( new LEDSet(LEDColor.RED), - new ConeReady() - .setWristVelocityTolerance(25) - .setShoulderVelocityTolerance(45) - .withTolerance(7, 9) - .withTimeout(4) + new ConeAutonReady() + .withTimeout(3) ); addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java index 36843be8..99b43114 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBlue.java @@ -55,26 +55,28 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { .setWristTolerance(23).setShoulderTolerance(20)); } } - private class ArmReadyBOOM extends ArmRoutine { - public ArmReadyBOOM() { - super(Manager.getInstance()::getReadyTrajectory); + + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(() -> { + if (Manager.getInstance().getNodeLevel() == NodeLevel.HIGH) + return new ArmState(-179, 136 - 8); + else + return new ArmState(-161.7, 133.9); + }); } - @Override + @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); - return new ArmTrajectory() - .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) - .setWristTolerance(30)) .addState( - new ArmState(dest.getShoulderDegrees(), wristSafeAngle).setWristLimp(true).setWristTolerance(360)) - .addState(dest); - - // return new ArmTrajectory().addState(dest); + new ArmState(dest.getShoulderDegrees(), src.getWristDegrees()) + .setWristLimp(true)) + .addState(dest); } } + private class FastStow extends ArmRoutine { public FastStow() { super(() -> new ArmState(-90, 90) @@ -118,10 +120,7 @@ public TwoPieceDockBlue() { // score first piece addCommands( new LEDSet(LEDColor.RED), - new ArmReady() - .withTolerance(7, 9) - .setShoulderVelocityTolerance(25) - .setWristVelocityTolerance(45) + new ConeAutonReady() .withTimeout(3) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java index a634f668..22df3a71 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockRed.java @@ -55,23 +55,24 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { .setWristTolerance(23).setShoulderTolerance(20)); } } - private class ArmReadyBOOM extends ArmRoutine { - public ArmReadyBOOM() { - super(Manager.getInstance()::getReadyTrajectory); + + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(() -> { + if (Manager.getInstance().getNodeLevel() == NodeLevel.HIGH) + return new ArmState(-179, 136 - 8); + else + return new ArmState(-161.7, 133.9); + }); } - @Override + @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); - return new ArmTrajectory() - .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) - .setWristTolerance(30)) .addState( - new ArmState(dest.getShoulderDegrees(), wristSafeAngle).setWristLimp(true).setWristTolerance(360)) - .addState(dest); - - // return new ArmTrajectory().addState(dest); + new ArmState(dest.getShoulderDegrees(), src.getWristDegrees()) + .setWristLimp(true)) + .addState(dest); } } @@ -118,10 +119,7 @@ public TwoPieceDockRed() { // score first piece addCommands( new LEDSet(LEDColor.RED), - new ArmReady() - .withTolerance(7, 9) - .setShoulderVelocityTolerance(25) - .setWristVelocityTolerance(45) + new ConeAutonReady() .withTimeout(3) ); From fdedbf92558ebf7c8ae29654fdda200ba174166d Mon Sep 17 00:00:00 2001 From: BenG49 Date: Thu, 19 Oct 2023 22:31:11 -0400 Subject: [PATCH 6/8] Update RobotContainer --- src/main/java/com/stuypulse/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 789698b0..4a4c1742 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -228,17 +228,17 @@ private void configureOperatorBindings() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothing()); - // autonChooser.addOption("Mobility", new Mobility()); + autonChooser.addOption("Mobility", new Mobility()); // One Piece - autonChooser.addOption("One Piece + Dock", new OnePieceDock()); - autonChooser.addOption("One Piece Mobility + Dock", new OnePieceMobilityDock()); + // autonChooser.addOption("One Piece Dock", new OnePieceDock()); + autonChooser.addOption("One Piece Mobility Dock", new OnePieceMobilityDock()); // One Piece Dock // autonChooser.addOption("1.5 Piece Dock", new OnePiecePickupDock()); - // autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockBump()); + // autonChooser.addOption("1.5 Piece Dock Bump", new OnePiecePickupDockBump()); // Two Piece From 80f445745349d0569d5696a92f4b536baeb57ccd Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 20 Oct 2023 20:39:20 -0400 Subject: [PATCH 7/8] Fix field object trajectory mirroring --- .../robot/commands/swerve/SwerveDriveFollowTrajectory.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFollowTrajectory.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFollowTrajectory.java index 9312951d..54cb0df1 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFollowTrajectory.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFollowTrajectory.java @@ -108,7 +108,7 @@ public void initialize() { )); } - trajectory.setTrajectory(path); + trajectory.setTrajectory(PathPlannerTrajectory.transformTrajectoryForAlliance(path, DriverStation.getAlliance())); super.initialize(); } From 94de14a5a976ec3c175091bcb1afd9f716d86b7d Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 20 Oct 2023 20:43:31 -0400 Subject: [PATCH 8/8] Fix comment --- .../java/com/stuypulse/robot/commands/auton/ThreePieceOld.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java index 45b0ddd5..df79dd12 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java @@ -29,7 +29,7 @@ import com.pathplanner.lib.PathPlanner; /** - * Old three piece -- scores cone high first + * Old three piece -- scores cone mid first */ public class ThreePieceOld extends DebugSequentialCommandGroup {