diff --git a/src/main/deploy/pathplanner/1 Piece Mobility Dock.path b/src/main/deploy/pathplanner/1 Piece Mobility Dock.path index a8153392..ae928249 100644 --- a/src/main/deploy/pathplanner/1 Piece Mobility Dock.path +++ b/src/main/deploy/pathplanner/1 Piece Mobility Dock.path @@ -2,12 +2,12 @@ "waypoints": [ { "anchorPoint": { - "x": 2.11, + "x": 1.956147399839285, "y": 2.2 }, "prevControl": null, "nextControl": { - "x": 3.1100000000000003, + "x": 2.9561473998392858, "y": 2.2 }, "holonomicAngle": 0.0, @@ -49,15 +49,15 @@ }, { "anchorPoint": { - "x": 6.27, + "x": 6.1694, "y": 2.21 }, "prevControl": { - "x": 6.4868342815896725, + "x": 6.386234281589673, "y": 2.21 }, "nextControl": { - "x": 6.4868342815896725, + "x": 6.386234281589673, "y": 2.21 }, "holonomicAngle": 0.0, diff --git a/src/main/deploy/pathplanner/3 Piece Blue.path b/src/main/deploy/pathplanner/3 Piece Blue.path index a2130db3..cade36be 100644 --- a/src/main/deploy/pathplanner/3 Piece Blue.path +++ b/src/main/deploy/pathplanner/3 Piece Blue.path @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 6.340000000000001, - "y": 4.44 + "y": 4.74 }, "prevControl": { "x": 5.587270042913402, - "y": 4.44 + "y": 4.74 }, "nextControl": { "x": 6.549202411287385, - "y": 4.44 + "y": 4.74 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 6.7700000000000005, - "y": 4.44 + "y": 4.74 }, "prevControl": { "x": 6.651547629778109, - "y": 4.471296466187375 + "y": 4.771296466187375 }, "nextControl": { "x": 6.651547629778109, - "y": 4.471296466187375 + "y": 4.771296466187375 }, "holonomicAngle": 0, "isReversal": true, @@ -100,15 +100,15 @@ { "anchorPoint": { "x": 6.58, - "y": 3.53 + "y": 3.73 }, "prevControl": { "x": 5.370212645435206, - "y": 4.73978735456479 + "y": 4.93978735456479 }, "nextControl": { "x": 6.712618407320928, - "y": 3.3973815926790722 + "y": 3.5973815926790722 }, "holonomicAngle": -30.0, "isReversal": false, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 35c7751b..90b0aa37 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -154,8 +154,7 @@ private void configureDriverBindings() { .debounce(0.5, DebounceType.kFalling) .onTrue(new LEDSet(LEDColor.RED)) .onTrue(new InstantCommand(() -> driver.setRumble(0.5))) - .onFalse(new InstantCommand(() -> driver.setRumble(0.0))) - ; + .onFalse(new InstantCommand(() -> driver.setRumble(0.0))); } private void configureOperatorBindings() { @@ -224,6 +223,12 @@ private void configureOperatorBindings() { // arm to neutral operator.getDPadRight().onTrue(new IntakeAcquire()) .onFalse(new IntakeStop()); + + new Trigger(intake::hasGamePiece) + .and(DriverStation::isTeleop) + .debounce(0.5, DebounceType.kFalling) + .onTrue(new InstantCommand(() -> operator.setRumble(0.5))) + .onFalse(new InstantCommand(() -> operator.setRumble(0.0))); } /**************/ 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 d95a6579..8c8fc521 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -12,7 +12,6 @@ import com.stuypulse.robot.commands.plant.PlantEngage; import com.stuypulse.robot.commands.swerve.*; import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay; -import com.stuypulse.robot.subsystems.Manager; import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.util.ArmState; import com.stuypulse.robot.util.ArmTrajectory; @@ -27,26 +26,6 @@ public class OnePieceMobilityDock 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 - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - return new ArmTrajectory() - .addState( - new ArmState(dest.getShoulderDegrees(), src.getWristDegrees()) - .setWristLimp(true)) - .addState(dest); - } - } - private class FastStow extends ArmRoutine { public FastStow() { super(() -> new ArmState(-90, 90) @@ -70,13 +49,13 @@ 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" ); // initial setup addCommands( - new ManagerSetNodeLevel(NodeLevel.MID), + new ManagerSetNodeLevel(NodeLevel.HIGH), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); @@ -84,7 +63,10 @@ public OnePieceMobilityDock() { // score first piece addCommands( new LEDSet(LEDColor.RED), - new ConeAutonReady() + new ArmReady() + .setWristVelocityTolerance(25) + .setShoulderVelocityTolerance(45) + .withTolerance(7, 9) .withTimeout(3) ); 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 8a7f59bb..ea3e2efd 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceBlue.java @@ -74,7 +74,7 @@ public ArmIntakeFirst() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { dest = new ArmState( - -70.82, + -70.82 - 2, 11); // 8.37); 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 5a875860..7c95abb8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceRed.java @@ -74,7 +74,7 @@ public ArmIntakeFirst() { @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { dest = new ArmState( - -70.82, + -70.82 - 2, 11); // 8.37); diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 5edb0ac3..c08c22db 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -15,10 +15,10 @@ public interface ArmTrajectories { public interface Acquire { ArmState kCone = new ArmState( - new SmartNumber("Arm Trajectories/Acquire Cone Shoulder", -75), - new SmartNumber("Arm Trajectories/Acquire Cone Wrist", 12)); + new SmartNumber("Arm Trajectories/Acquire Cone Shoulder", -78), + new SmartNumber("Arm Trajectories/Acquire Cone Wrist", 11)); ArmState kCube = new ArmState( - new SmartNumber("Arm Trajectories/Acquire Cube Shoulder", -75), + new SmartNumber("Arm Trajectories/Acquire Cube Shoulder", -78), new SmartNumber("Arm Trajectories/Acquire Cube Wrist", 12)); ArmState kHPCone = new ArmState( diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 97b1edc6..1db1cf5c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -122,7 +122,7 @@ public interface Drive { public interface FrontRight { String ID = "Front Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(122.605534) // recalibrated 10/1 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(122.949092) // recalibrated 11/11 .plus(Rotation2d.fromDegrees(0)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5); }