holding on a prayer rn by adding back repositionGamePiece

FRCTeam3255 · Oct 11, 2024 · 03583bf · 03583bf
1 parent 5954afe
commit 03583bf
Show file tree

Hide file tree

Showing 11 changed files with 58 additions and 46 deletions.
diff --git a/src/main/deploy/pathplanner/paths/U PsC5.path b/src/main/deploy/pathplanner/paths/U PsC5.path
@@ -16,16 +16,16 @@
     },
     {
       "anchor": {
-        "x": 0.973104168430559,
-        "y": 2.4440782784699477
+        "x": 1.9838583821311786,
+        "y": 2.570027482397308
       },
       "prevControl": {
-        "x": 0.8795509328910394,
-        "y": 3.2275866261134243
+        "x": 1.530867926600718,
+        "y": 2.912081499838676
       },
       "nextControl": {
-        "x": 1.1164128764858732,
-        "y": 1.2438678485066876
+        "x": 2.909246987953023,
+        "y": 1.8712646575930587
       },
       "isLocked": false,
       "linkedName": null

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -174,13 +174,10 @@ public RobotContainer() {
   }
 
   private void configureDriverBindings(SN_XboxController controller) {
-    controller.btn_North.onTrue(Commands.runOnce(() -> subDrivetrain.resetDriving(FieldConstants.ALLIANCE)));
-
     // Reset Pose
     controller.btn_South
         .onTrue(Commands.runOnce(
-            () -> subDrivetrain.resetPoseToPose(FieldConstants.GET_FIELD_POSITIONS().get()[6].toPose2d()))
-            .alongWith(Commands.runOnce(() -> subDrivetrain.resetDriving(FieldConstants.ALLIANCE))));
+            () -> subDrivetrain.resetPoseToPose(FieldConstants.GET_FIELD_POSITIONS().get()[6].toPose2d())));
 
     controller.btn_LeftTrigger
         .onTrue(Commands.runOnce(() -> subIntake.setPivotAngle(prefIntake.pivotGroundIntakeAngle.getValue())))
@@ -208,8 +205,6 @@ private void configureOperatorBindings(SN_XboxController controller) {
         .whileTrue(new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber)
             .unless(() -> subTransfer.hasGamePiece));
 
-    repositionTrigger.onTrue(new RepositionGamePiece(subTransfer, subShooter));
-
     // Left Bumper = Enable both Manuals
     // Left Stick = Manual Hood
     // Right Stick = Manual Turret

diff --git a/src/main/java/frc/robot/commands/IntakeGroundGamePiece.java b/src/main/java/frc/robot/commands/IntakeGroundGamePiece.java
@@ -80,6 +80,7 @@ public void end(boolean interrupted) {
       subIntake.setRollerNeutralOutput();
     }
     if (!interrupted) {
+      subTransfer.repositionGamePiece();
       subShooter.setDesiredVelocities(prefShooter.leftShooterSubVelocity.getValue(),
           prefShooter.rightShooterSubVelocity.getValue());
       subShooter.getUpToSpeed();

diff --git a/src/main/java/frc/robot/commands/autos/Centerline.java b/src/main/java/frc/robot/commands/autos/Centerline.java
@@ -62,8 +62,6 @@ public Centerline(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pitc
     addCommands(
         Commands.runOnce(
             () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
-        Commands.runOnce(() -> subDrivetrain.resetYaw(
-            getInitialPose().get().getRotation().getDegrees())),
 
         Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),
             prefShooter.rightShooterSpeakerVelocity.getValue())),

diff --git a/src/main/java/frc/robot/commands/autos/DefaultAuto.java b/src/main/java/frc/robot/commands/autos/DefaultAuto.java
@@ -45,8 +45,6 @@ public DefaultAuto(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
     addCommands(
         Commands.runOnce(
             () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
-        Commands.runOnce(() -> subDrivetrain.resetYaw(
-            getInitialPose().get().getRotation().getDegrees())),
 
         new PathPlannerAuto(determinePathName()));
   }

diff --git a/src/main/java/frc/robot/commands/autos/PreloadOnly.java b/src/main/java/frc/robot/commands/autos/PreloadOnly.java
@@ -78,12 +78,6 @@ public PreloadOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
         Commands.runOnce(
             () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
 
-        Commands.runOnce(() -> subDrivetrain.resetYaw(
-            getInitialPose().get().getRotation().getDegrees())).unless(() -> FieldConstants.isRedAlliance()),
-
-        Commands.runOnce(() -> subDrivetrain.resetYaw(
-            getInitialPose().get().getRotation().getDegrees() - 180)).unless(() -> !FieldConstants.isRedAlliance()),
-
         Commands.sequence(
             Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),
                 prefShooter.rightShooterSpeakerVelocity.getValue())),
@@ -92,9 +86,6 @@ public PreloadOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
             Commands.runOnce(() -> subTransfer.setTransferSensorAngle(0)),
             Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false)),
 
-            // Intake until we have the game piece
-            new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber),
-
             // PRELOAD
             // Aim
             Commands.parallel(

diff --git a/src/main/java/frc/robot/commands/autos/PreloadTaxi.java b/src/main/java/frc/robot/commands/autos/PreloadTaxi.java
@@ -67,8 +67,6 @@ public PreloadTaxi(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
     addCommands(
         Commands.runOnce(
             () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
-        Commands.runOnce(() -> subDrivetrain.resetYaw(
-            getInitialPose().get().getRotation().getDegrees())),
 
         Commands.sequence(
             Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),
@@ -78,9 +76,6 @@ public PreloadTaxi(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pit
             Commands.runOnce(() -> subTransfer.setTransferSensorAngle(0)),
             Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false)),
 
-            // Intake until we have the game piece
-            new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber),
-
             // PRELOAD
             // Aim
             Commands.parallel(

diff --git a/src/main/java/frc/robot/commands/autos/WingOnly.java b/src/main/java/frc/robot/commands/autos/WingOnly.java
@@ -11,11 +11,16 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.LockedLocation;
+import frc.robot.RobotPreferences.prefIntake;
+import frc.robot.RobotPreferences.prefPitch;
 import frc.robot.RobotPreferences.prefShooter;
+import frc.robot.RobotPreferences.prefTransfer;
+import frc.robot.RobotPreferences.prefTurret;
 import frc.robot.FieldConstants;
 import frc.robot.RobotContainer;
 import frc.robot.commands.AimAuto;
@@ -62,8 +67,6 @@ public WingOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pitch
     addCommands(
         Commands.runOnce(
             () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
-        Commands.runOnce(() -> subDrivetrain.resetYaw(
-            getInitialPose().get().getRotation().getDegrees())),
 
         Commands.runOnce(() -> subShooter.setDesiredVelocities(prefShooter.leftShooterSpeakerVelocity.getValue(),
             prefShooter.rightShooterSpeakerVelocity.getValue())),
@@ -73,6 +76,35 @@ public WingOnly(Drivetrain subDrivetrain, Intake subIntake, LEDs subLEDs, Pitch
         Commands.runOnce(() -> subShooter.setIgnoreFlywheelSpeed(false)),
 
         // Intake until we have the game piece
+        // this hurts. im so sorry. the intaking command randomly broke. do not follow
+        // this example. i have officially lost it
+        // Commands.parallel(
+        // Commands.runOnce(() ->
+        // subTurret.setTurretAngle(prefTurret.turretIntakePos.getValue())),
+        // Commands.runOnce(
+        // () ->
+        // subPitch.setPitchAngle(Units.rotationsToDegrees(prefPitch.pitchReverseLimit.getValue()))),
+        // Commands.runOnce(() ->
+        // subIntake.setPivotAngle(prefIntake.pivotGroundIntakeAngle.getValue())),
+        // Commands.runOnce(() ->
+        // subIntake.setIntakeRollerSpeed(prefIntake.rollerIntakeSpeed.getValue())),
+        // Commands
+        // .runOnce(() ->
+        // subTransfer.setTransferMotorSpeed(prefTransfer.transferIntakeGroundSpeed.getValue())),
+        // Commands.runOnce(() ->
+        // subTransfer.setFeederMotorSpeed(prefTransfer.feederIntakeGroundSpeed.getValue()))),
+
+        // Commands.waitUntil(() -> subTransfer.calcGamePieceCollected(false)),
+
+        // Commands.parallel(
+        // Commands.runOnce(() -> subIntake.setRollerNeutralOutput()),
+        // Commands.runOnce(() ->
+        // subShooter.setDesiredVelocities(prefShooter.leftShooterSubVelocity.getValue(),
+        // prefShooter.rightShooterSubVelocity.getValue())),
+        // Commands.runOnce(() -> subShooter.getUpToSpeed()),
+        // Commands.runOnce(() -> subTransfer.setTransferNeutralOutput()),
+        // Commands.runOnce(() -> subTransfer.setFeederNeutralOutput())),
+
         new IntakeGroundGamePiece(subIntake, subTransfer, subTurret, subPitch, subShooter, subClimber),
 
         // PRELOAD

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -242,18 +242,6 @@ public double getGyroRate() {
     return pigeon.getRate();
   }
 
-  /**
-   * Resets the driving orientation depending on alliance.
-   * 
-   * @param alliance
-   */
-  public void resetDriving(Optional<Alliance> alliance) {
-    if (alliance.isPresent() && alliance.get().equals(Alliance.Red)) {
-      resetYaw();
-    }
-    resetYaw(180);
-  }
-
   @Override
   public void periodic() {
     super.periodic();

diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java
@@ -180,6 +180,20 @@ public void setTransferSensorAngle(double angle) {
     transferMotor.setPosition(Units.degreesToRotations(angle));
   }
 
+  public void repositionGamePiece() {
+    hasRepositioned = false;
+    double time = Timer.getFPGATimestamp();
+    while (Timer.getFPGATimestamp() <= time + prefTransfer.transferRepositionTime.getValue()) {
+      setTransferMotorSpeed(prefTransfer.transferRepositionSpeed.getValue());
+    }
+    time = Timer.getFPGATimestamp();
+    while (Timer.getFPGATimestamp() <= time + prefTransfer.transferRepositionTime.getValue() / 2) {
+      setTransferMotorSpeed(-prefTransfer.transferRepositionSpeed.getValue());
+    }
+    setTransferNeutralOutput();
+    hasRepositioned = true;
+  }
+
   @Override
   public void periodic() {
     SmartDashboard.putNumber("Transfer/Feeder/Percent", getFeederMotorPercentOutput());

diff --git a/vendordeps/SuperCORE.json b/vendordeps/SuperCORE.json
@@ -1,7 +1,7 @@
 {
     "fileName": "SuperCORE.json",
     "name": "SuperCORE",
-    "version": "2024.4.3",
+    "version": "2024.4.6",
     "frcYear": 2024,
     "uuid": "f82a9412-6f53-4bc0-960c-6895a5236f57",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.frcteam3255",
             "artifactId": "supercore",
-            "version": "2024.4.3"
+            "version": "2024.4.6"
         }
     ],
     "jniDependencies": [],