DCMP stuff

AEMBOT · Apr 21, 2023 · c185274 · c185274
1 parent e2ec5ab
commit c185274
Show file tree

Hide file tree

Showing 6 changed files with 160 additions and 5 deletions.
diff --git a/src/main/deploy/pathplanner/StraightBackDock2.path b/src/main/deploy/pathplanner/StraightBackDock2.path
@@ -0,0 +1,138 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.9326182242108072,
+        "y": 2.7862770255455045
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.932618224210806,
+        "y": 2.7862770255455045
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "placeConeHigh"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "deadline",
+        "waitTime": 2.0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.4442414369883845,
+        "y": 2.7748224237729175
+      },
+      "prevControl": {
+        "x": 2.286192799861264,
+        "y": 2.776990865470345
+      },
+      "nextControl": {
+        "x": 2.5947502310915618,
+        "y": 2.772757429408554
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 5.7,
+        "y": 2.7748578574080227
+      },
+      "prevControl": {
+        "x": 4.70072705402327,
+        "y": 2.736732020512706
+      },
+      "nextControl": {
+        "x": 5.911955948842143,
+        "y": 2.782944734939415
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 5.447235112958647,
+        "y": 2.773026206452805
+      },
+      "prevControl": {
+        "x": 6.112998729686881,
+        "y": 2.776005562558714
+      },
+      "nextControl": {
+        "x": 4.669926164890694,
+        "y": 2.7695476742795013
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.930972648270083,
+        "y": 2.7748578574080227
+      },
+      "prevControl": {
+        "x": 4.533745161507415,
+        "y": 2.837663282164171
+      },
+      "nextControl": null,
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "autoDock"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "maxVelocity": 1.5,
+  "maxAcceleration": 1.0,
+  "isReversed": null,
+  "markers": [
+    {
+      "position": 0.01,
+      "names": [
+        "stowArm"
+      ]
+    }
+  ]
+}
diff --git a/src/main/deploy/pathplanner/twopieceNoDock-redLeft-blueRight.path b/src/main/deploy/pathplanner/twopieceNoDock-redLeft-blueRight.path
@@ -153,7 +153,7 @@
       ]
     },
     {
-      "position": 3.95,
+      "position": 3.99,
       "names": [
         "openIntake"
       ]

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -203,7 +203,7 @@ public static final class ArmConstants {
         public static final double extendOffset = 2.5;
         public static final double extendToGroundPickup = 0.20;
         public static final double extendToFloor = 0.10;
-        public static final double extendToMid = 0.50;
+        public static final double extendToMid = 0.53;
         public static final double extendToHigh = 1.05;
         public static final double extendToSubstation = 0.90;
         public static final double extendToSingleSubstation = 0.4;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -159,6 +159,11 @@ public RobotContainer() {
                 PathPlanner.loadPath("StraightBackDock", 1.5, 1)
         );
 
+        Command TwistBackDock = autoBuilder.fullAuto(
+                PathPlanner.loadPath("StraightBackDock2", 1.5, 1)
+        );
+
+
         Command redLeft_blueRight = autoBuilder.fullAuto(
                 PathPlanner.loadPath("redLeft-blueRight", maxVelMetersPerSec, maxAccelMetersPerSecondSq)
         );
@@ -232,6 +237,7 @@ public RobotContainer() {
         autoSelector.addOption("twopieceNoDock-redLeft-blueRight", twopiece_NoDock_redLeft_blueRight);
         autoSelector.addOption("twoHalf-redLeft-blueRight", twoHalf_redLeft_blueRight);
         autoSelector.addOption("straightBackDock", StraightBackDock);
+        autoSelector.addOption("twistBackDock", TwistBackDock);
         // autoSelector.addOption("twopiece-redRight-blueLeft", twopiece_redRight_blueLeft);
         autoSelector.addOption("MobilityRedLeft-BlueRight", basicRedLeft_blueRight);
         autoSelector.addOption("MobilityRedRight-BlueLeft", basicRedRight_blueLeft);

diff --git a/src/main/java/frc/robot/commands/arm/ArmCommands.java b/src/main/java/frc/robot/commands/arm/ArmCommands.java
@@ -81,7 +81,7 @@ public static Command getPlaceGamePieceCommand(DrivebaseS m_drivebase, ArmSubsys
                                                         )),
                                                         finalTargetPosition.plus(ONE_METER_BACK.times(
                                                                 switch (position) {
-                                                                    case DOUBLE_SUBSTATION -> 0.17;
+                                                                    case DOUBLE_SUBSTATION -> 0;
                                                                     default -> 0.38;
                                                                 }
                                                         ))
@@ -95,7 +95,7 @@ public static Command getPlaceGamePieceCommand(DrivebaseS m_drivebase, ArmSubsys
                         m_drivebase.chasePoseC(
                                 () -> finalTargetPosition.plus(ONE_METER_BACK.times(
                                         switch (position) {
-                                            case DOUBLE_SUBSTATION -> 0.17;
+                                            case DOUBLE_SUBSTATION -> 0;
                                             default -> 0.38;
                                         }
                                 )),

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -22,7 +22,18 @@ public IntakeSubsystem() {
     }
 
     public Command getIntakeAutoClampCommand() {
-        return startEnd(this::openClamp, this::closeClamp).until(this::getLimitSwitchState);
+//        return startEnd(this::openClamp, this::closeClamp).until(this::getLimitSwitchState);
+        return new FunctionalCommand(
+                this::openClamp,
+                () -> {},
+                (interrupted) -> {
+                    if (!interrupted) {
+                        this.closeClamp();
+                    }
+                },
+                this::getLimitSwitchState,
+                this
+        );
     }
 
     @Override