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 98% rename from src/main/deploy/pathplanner/2 Piece Wire.path rename to src/main/deploy/pathplanner/2 Piece Bump.path index 365b0178..f73ad063 100644 --- a/src/main/deploy/pathplanner/2 Piece Wire.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 similarity index 97% rename from src/main/deploy/pathplanner/2 Piece + Dock Blue.path rename to 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 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 97% rename from src/main/deploy/pathplanner/2 Piece + Dock Red.path rename to 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/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 diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 4df6373d..4a4c1742 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 Bump", 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/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/OnePiecePickupDockWire.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockBump.java similarity index 95% 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..ed3bcad0 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,9 +36,9 @@ 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), + 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 cc2804dd..c33fcb7a 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) @@ -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/ThreePieceWLowRed.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowRed.java rename to src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java index 1ea5e058..8a7f59bb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowRed.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 ThreePieceWLowRed extends DebugSequentialCommandGroup { +public class ThreePieceBlue extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -149,10 +149,10 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public ThreePieceWLowRed() { + public ThreePieceBlue() { 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 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/ThreePieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBump.java similarity index 98% 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..4cddf7e4 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,10 +141,10 @@ 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), + 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/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java similarity index 80% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java rename to src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java index 2a0b0e1e..df79dd12 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceOld.java @@ -22,38 +22,44 @@ 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 ThreePieceWLow extends DebugSequentialCommandGroup { +/** + * Old three piece -- scores cone mid first + */ +public class ThreePieceOld extends DebugSequentialCommandGroup { - static class AutonMidCubeReady extends ArmRoutine { - public AutonMidCubeReady() { - super(() -> Ready.Mid.kAutonCubeBack); + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(Manager.getInstance()::getReadyTrajectory); } @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)); + .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 AutonHighCubeReady extends ArmRoutine { - public AutonHighCubeReady() { - super(() -> Ready.High.kCubeAutonBack); + 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(); @@ -62,10 +68,9 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) .setWristTolerance(45)) .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) - .setWristTolerance(30).setShoulderTolerance(20)); + .setWristTolerance(7).setShoulderTolerance(25)); } } - static class ArmIntakeFirst extends ArmRoutine { public ArmIntakeFirst() { super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); @@ -108,14 +113,11 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { -70.82, 10); // 8.37); - // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); - double intermediateShoulderDegrees = -60; + double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); return new ArmTrajectory() - .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) - .setShoulderTolerance(694) - .setWristTolerance(15)) + // .addState(src.getShoulderDegrees(), wristSafeAngle) .addState( new ArmState(intermediateShoulderDegrees, wristSafeAngle) @@ -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 ThreePieceOld() { 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 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" ); @@ -160,14 +162,27 @@ public ThreePieceWLow() { // initial setup addCommands( - new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), - new ManagerSetNodeLevel(NodeLevel.LOW), + new ManagerSetNodeLevel(NodeLevel.MID), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); - // score first piece + intake second piece + // 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( @@ -180,10 +195,10 @@ public ThreePieceWLow() { .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) - .andThen(new IntakeAcquire()) - .andThen(new ArmIntakeFirst() - .withTolerance(4, 10)) + .andThen(new IntakeAcquire()), + new ArmIntakeFirst() + .withTolerance(4, 10) ), new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) @@ -197,13 +212,6 @@ public ThreePieceWLow() { 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), @@ -211,16 +219,14 @@ public ThreePieceWLow() { new LEDSet(LEDColor.RED), - new ParallelCommandGroup( - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop(), - - new WaitCommand(0.5).andThen(new IntakeStop()), + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), - new AutonMidCubeReady() - .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) + new AutonReady(), + new WaitCommand(0.5).andThen(new IntakeStop()) ), new ManagerSetGridNode(1), @@ -230,6 +236,13 @@ public ThreePieceWLow() { new IntakeStop() ); + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + // intake third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -279,7 +292,7 @@ public ThreePieceWLow() { paths.get("Score Third Piece")) .fieldRelative() .withStop() - .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), + .alongWith(new AutonReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), new ManagerSetGridNode(1), // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), @@ -298,4 +311,4 @@ public ThreePieceWLow() { .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/ThreePieceRed.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowBlue.java rename to src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java index 964b6a13..5a875860 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLowBlue.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 ThreePieceWLowBlue extends DebugSequentialCommandGroup { +public class ThreePieceRed extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -149,10 +149,10 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public ThreePieceWLowBlue() { + public ThreePieceRed() { 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 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/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java similarity index 87% 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..b79adfc5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceBump.java @@ -27,27 +27,26 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class TwoPieceWire extends DebugSequentialCommandGroup { - - private static class ConeReady extends ArmRoutine { - public ConeReady() { - super(Manager.getInstance()::getReadyTrajectory); +public class TwoPieceBump extends DebugSequentialCommandGroup { + + 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; @@ -61,9 +60,9 @@ 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), + PathPlanner.loadPathGroup("2 Piece Bump", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS), "Intake Piece", "Score Piece", "Back Away" ); @@ -80,11 +79,8 @@ public TwoPieceWire() { // 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 2656a490..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) @@ -102,7 +104,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" ); @@ -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/TwoPieceDockWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDockBump.java similarity index 93% 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..ae15a467 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,9 +30,9 @@ 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), + 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..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); } } @@ -102,7 +103,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" ); @@ -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) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireRed.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireRed.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java index 77f22693..40e178bc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpBlue.java @@ -30,7 +30,7 @@ import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; -public class BCThreePieceWLowWireRed extends DebugSequentialCommandGroup { +public class BCThreePieceBumpBlue extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -151,10 +151,10 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public BCThreePieceWLowWireRed() { + public BCThreePieceBumpBlue() { 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 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/BCThreePieceWLowWireBlue.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java similarity index 97% rename from src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireBlue.java rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceBumpRed.java index 73322a55..8db38e8a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BCThreePieceWLowWireBlue.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 BCThreePieceWLowWireBlue extends DebugSequentialCommandGroup { +public class BCThreePieceBumpRed extends DebugSequentialCommandGroup { static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { @@ -151,10 +151,10 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); - public BCThreePieceWLowWireBlue() { + public BCThreePieceBumpRed() { 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 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/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" 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(); }