Changes for/from Brunswick (#199)

* Brunswick init * One piece mobility dock revert * Move intaking shoulder in * Tune swerve * Add operator controller buzz * Try to fix wrist bouncing three piece * Shift three piece blue * Raise cube intaking (move shoulder in) --------- Co-authored-by: Prog694 <[email protected]>
StuyPulse · Nov 13, 2023 · 4f11a71 · 4f11a71
1 parent 4291dd2
commit 4f11a71
Show file tree

Hide file tree

Showing 8 changed files with 33 additions and 46 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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,21 +49,24 @@ 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)
         );
 
         // 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
@@ -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
@@ -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
@@ -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
@@ -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);
         }