MO Changes

RegisJesuitRobotics · Mar 12, 2024 · 243e556 · 243e556
1 parent 9c7cb96
commit 243e556
Show file tree

Hide file tree

Showing 5 changed files with 68 additions and 14 deletions.
diff --git a/src/main/deploy/pathplanner/autos/AmpSpeaker1NoteMobilityWAIT10.auto b/src/main/deploy/pathplanner/autos/AmpSpeaker1NoteMobilityWAIT10.auto
@@ -0,0 +1,43 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.74,
+      "y": 6.61
+    },
+    "rotation": 60.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ShootNote"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 10.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "AmpSpeaker-Mobility"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "Amp Speaker",
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/SourceSpeaker-Mobility.path b/src/main/deploy/pathplanner/paths/SourceSpeaker-Mobility.path
@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.1905904713351545,
-        "y": 1.3049178174275693
+        "x": 1.081360928601144,
+        "y": -0.0017036255330342698
       },
       "isLocked": false,
       "linkedName": "SourceSpeaker"
     },
     {
       "anchor": {
-        "x": 3.858855262194511,
-        "y": 1.022510230073842
+        "x": 7.26759438251701,
+        "y": 0.8251074008509334
       },
       "prevControl": {
-        "x": 3.2258727388154664,
-        "y": 1.0419866154085826
+        "x": 5.982933492584543,
+        "y": 0.7624410159761787
       },
       "nextControl": null,
       "isLocked": false,
@@ -32,14 +32,14 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.8,
+    "maxVelocity": 2.0,
     "maxAcceleration": 4.0,
     "maxAngularVelocity": 500.0,
     "maxAngularAcceleration": 2000.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -60.0,
+    "rotation": 0.0,
     "rotateFast": false
   },
   "reversed": false,
@@ -48,5 +48,5 @@
     "rotation": -60.0,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -19,8 +19,8 @@ public final class Constants {
   private Constants() {}
 
   public static class SetpointConstants {
-    public static final double AMP_ELEVATOR_HEIGHT = Units.inchesToMeters(3.5);
-    public static final double AMP_WRIST_ANGLE_RADIANS = Units.degreesToRadians(105.0);
+    public static final double AMP_ELEVATOR_HEIGHT = Units.inchesToMeters(3.0);
+    public static final double AMP_WRIST_ANGLE_RADIANS = Units.degreesToRadians(120.0);
 
     public static final double INTAKE_ELEVATOR_HEIGHT = Units.inchesToMeters(0);
     public static final double INTAKE_WRIST_ANGLE_RADIANS = Units.degreesToRadians(0.0);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -51,8 +51,7 @@ public class RobotContainer {
 //  private final PhotonSubsystem photonSubsystem = new PhotonSubsystem();
 //    private final SwerveDriveSubsystem driveSubsystem =
 //        new SwerveDriveSubsystem(photonSubsystem::getEstimatedGlobalPose);
-  private final SwerveDriveSubsystem driveSubsystem 
-  = new SwerveDriveSubsystem((pose) -> List.of());
+  private final SwerveDriveSubsystem driveSubsystem = new SwerveDriveSubsystem((pose) -> List.of());
   private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
   private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
   private final WristSubsystem wristSubsystem = new WristSubsystem();
@@ -168,6 +167,7 @@ private void configureDriverBindings() {
             transportSubsystem.setVoltageCommand(
                 Constants.TransportConstants.TRANSPORT_CLOSE_SPEAKER_VOLTAGE));
     driverController.minus().whileTrue(new LockModulesCommand(driveSubsystem).repeatedly());
+    driverController.leftBumper().whileTrue(IntakingCommands.intakeUntilDetectedNoSlap(intakeSubsystem, transportSubsystem));
     driverController
         .rightTrigger()
         .whileTrue(
@@ -177,6 +177,7 @@ private void configureDriverBindings() {
             slapdownSuperstructure.setUpCommand()
         );
     driverController.circle().whileTrue(new LockModulesCommand(driveSubsystem).repeatedly());
+    driverController.y().whileTrue(Commands.parallel(transportSubsystem.setVoltageCommand(-8), shooterSubsystem.setVoltageCommand(-8)));
 
     driverController.x().onTrue(Commands.runOnce(() -> signalHumanPlayer.set(true))).onFalse(
         Commands.sequence(Commands.waitSeconds(1.5), Commands.runOnce(() -> signalHumanPlayer.set(false)))
@@ -189,7 +190,7 @@ private void configureOperatorBindings() {
         .onTrue(
             Commands.parallel(
                 ScoringCommands.shootSetpointAmpCommand(shooterSubsystem),
-                transportSubsystem.setVoltageCommand(10)));
+                Commands.sequence(Commands.waitSeconds(0.75),transportSubsystem.setVoltageCommand(10))));
     operatorController
         .triangle()
         .onTrue(

diff --git a/src/main/java/frc/robot/commands/IntakingCommands.java b/src/main/java/frc/robot/commands/IntakingCommands.java
@@ -21,4 +21,14 @@ public static Command intakeUntilDetected(
         .unless(transport::atSensor)
         .andThen(Commands.parallel(intake.stopCommand(), transport.stopMovementCommand()));
   }
+
+  public static Command intakeUntilDetectedNoSlap(
+      IntakeSubsystem intake, TransportSubsystem transport) {
+    return Commands.parallel(
+            intake.setIntakeVoltageCommand(IntakeConstants.INTAKE_VOLTAGE),
+            transport.setVoltageCommand(TransportConstants.TRANSPORT_LOAD_VOLTAGE))
+        .until(transport::atSensor)
+        .unless(transport::atSensor)
+        .andThen(Commands.parallel(intake.stopCommand(), transport.stopMovementCommand()));
+  }
 }