wait whoops

BitBucketsFRC4183 · Mar 7, 2025 · 4374154 · 4374154
1 parent 9718b06
commit 4374154
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/OperatorInput.java b/src/main/java/frc/robot/OperatorInput.java
@@ -35,6 +35,8 @@ public class OperatorInput {
   public final Trigger movementDesired = movementNotDesired.negate();
   public final Trigger resetHeading = driver.start();
 
+  // dpad not pressed
+  public static final Trigger alignmentRobotRelative = driver.povCenter().negate();
   //operator controls
   final Trigger elevatorGoToOrigin = operator.a();
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -34,13 +34,12 @@
 import frc.robot.commands.ArmCommands.ArmHoverCommand;
 import frc.robot.commands.ArmCommands.BendCommand;
 import frc.robot.commands.ArmCommands.ManualArmCommand;
-import frc.robot.commands.BaseDriveCommand;
+import frc.robot.commands.RobotRelativeDriveCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorGoToOriginCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorSetPointCommand;
 import frc.robot.commands.ElevatorCommands.ManualElevatorCommand;
 import frc.robot.commands.ElevatorCommands.ResetElevatorEncoderCommand;
 import frc.robot.commands.FieldRelativeDriveCommand;
-import frc.robot.commands.IntakeCommands.IntakeSetPivotCommand;
 import frc.robot.commands.IntakeCommands.IntakeSetRollersCommand;
 import frc.robot.commands.ResetHeadingCommand;
 import frc.robot.constants.ArmConstants;
@@ -73,6 +72,8 @@
 import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoral;
 import org.littletonrobotics.junction.Logger;
 
+import java.util.function.DoubleSupplier;
+
 
 public class RobotContainer {
   // Subsystems
@@ -254,6 +255,28 @@ void loadCommands() {
                 () -> slewTheta.calculate(-driveController.getRightX()),
                     driveSubsystem::getRotation
     ));
+
+    DoubleSupplier dPadX = () -> {
+      if (driveController.povLeft().getAsBoolean()) {
+        return -1.0;
+      } else if (driveController.povRight().getAsBoolean()) {
+        return 1.0;
+      } else { return 0.0;}
+    };
+
+    DoubleSupplier dPadY = () -> {
+      if (driveController.povDown().getAsBoolean()) {
+        return -1.0;
+      } else if (driveController.povUp().getAsBoolean()) {
+        return 1.0;
+      } else { return 0.0;}
+    };
+
+    OperatorInput.alignmentRobotRelative.whileTrue(
+            new RobotRelativeDriveCommand(
+                    driveSubsystem,dPadX, dPadY,() -> 0
+            )
+    );
   }
 
 

diff --git a/src/main/java/frc/robot/commands/RobotRelativeDriveCommand.java b/src/main/java/frc/robot/commands/RobotRelativeDriveCommand.java
@@ -6,24 +6,23 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.OperatorInput;
 import frc.robot.constants.DriveConstants;
 import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
 
 import java.util.function.DoubleSupplier;
 
-public class BaseDriveCommand extends Command {
+public class RobotRelativeDriveCommand extends Command {
         private final DriveSubsystem drive;
         private final DoubleSupplier xSupplier;
         private final DoubleSupplier ySupplier;
         private final DoubleSupplier omegaSupplier;
 
-        public BaseDriveCommand(DriveSubsystem drive,
-                                DoubleSupplier xSupplier,
-                                DoubleSupplier ySupplier,
-                                DoubleSupplier omegaSupplier) {
+        public RobotRelativeDriveCommand(DriveSubsystem drive,
+                                         DoubleSupplier xSupplier,
+                                         DoubleSupplier ySupplier,
+                                         DoubleSupplier omegaSupplier) {
             this.drive = drive;
             this.xSupplier = xSupplier;
             this.ySupplier = ySupplier;

diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java
@@ -37,6 +37,8 @@ public class DriveConstants {
     public static final double turboSpeed =  maxSpeedMetersPerSec;
     public static final double normalSpeed = maxSpeedMetersPerSec - 1;
     public static final double slowSpeed = maxSpeedMetersPerSec - 2;
+    public static final double alignmentSpeed = 1;
+
     public static final double odometryFrequency = 100.0; // Hz
     public static final double trackWidth = Units.inchesToMeters(26.5);
     public static final double wheelBase = Units.inchesToMeters(26.5);