From 03583bfbf02c45eaba664b5c2f78674cac5d0075 Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Thu, 10 Oct 2024 22:51:11 -0700 Subject: [PATCH] holding on a prayer rn by adding back repositionGamePiece --- src/main/deploy/pathplanner/paths/U PsC5.path | 12 +++---- src/main/java/frc/robot/RobotContainer.java | 7 +--- .../robot/commands/IntakeGroundGamePiece.java | 1 + .../frc/robot/commands/autos/Centerline.java | 2 -- .../frc/robot/commands/autos/DefaultAuto.java | 2 -- .../frc/robot/commands/autos/PreloadOnly.java | 9 ----- .../frc/robot/commands/autos/PreloadTaxi.java | 5 --- .../frc/robot/commands/autos/WingOnly.java | 36 +++++++++++++++++-- .../java/frc/robot/subsystems/Drivetrain.java | 12 ------- .../java/frc/robot/subsystems/Transfer.java | 14 ++++++++ vendordeps/SuperCORE.json | 4 +-- 11 files changed, 58 insertions(+), 46 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/U PsC5.path b/src/main/deploy/pathplanner/paths/U PsC5.path index 9bd9d14..d182d4a 100644 --- 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 index 6447769..b8ea423 100644 --- 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 index 96931aa..a77a4ce 100644 --- 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 index 35955dc..b307862 100644 --- 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 index 6f2ba3e..28c0964 100644 --- 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 index ff938e9..b518130 100644 --- 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 index 3b98863..1129b3e 100644 --- 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 index 3dd0190..6c12377 100644 --- 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 index 82c6dbf..0f9e24a 100644 --- 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) { - 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 index 8294ee6..bf88d05 100644 --- 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 index 48e2570..9589e34 100644 --- 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": [],