From c18527484af352789d7d45db21d6b92992ee7376 Mon Sep 17 00:00:00 2001 From: Wither <86643519+ItsDaWither@users.noreply.github.com> Date: Thu, 20 Apr 2023 17:26:58 -0700 Subject: [PATCH] DCMP stuff --- .../deploy/pathplanner/StraightBackDock2.path | 138 ++++++++++++++++++ .../twopieceNoDock-redLeft-blueRight.path | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 + .../frc/robot/commands/arm/ArmCommands.java | 4 +- .../frc/robot/subsystems/IntakeSubsystem.java | 13 +- 6 files changed, 160 insertions(+), 5 deletions(-) create mode 100644 src/main/deploy/pathplanner/StraightBackDock2.path diff --git a/src/main/deploy/pathplanner/StraightBackDock2.path b/src/main/deploy/pathplanner/StraightBackDock2.path new file mode 100644 index 0000000..70c862e --- /dev/null +++ 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" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/twopieceNoDock-redLeft-blueRight.path b/src/main/deploy/pathplanner/twopieceNoDock-redLeft-blueRight.path index c40b2da..3818de5 100644 --- 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 index 7060e55..ba08de3 100644 --- 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 index 350d629..16d26fd 100644 --- 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 index 4beb7ea..6aa401a 100644 --- 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 index ca836b2..94f4b7e 100644 --- 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