a

diff --git a/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java b/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java
new file mode 100644 (file)
index 0000000..2365a9c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java
@@ -0,0 +1,30 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake.Intake;
+
+public class IntakeMovementCommand extends Command {
+    private final Intake intake;
+    private final double interval = 0.6; // Change this to make it faster/slower (seconds)
+
+    public IntakeMovementCommand(Intake intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void execute() {
+        intake.spinStop();
+        if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) {
+            intake.extend(); 
+        } else {
+            intake.intermediateExtend();
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        
+    }
+}
\ No newline at end of file

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index ddc0e40c217bf44038027af07b8866ce3a7cb5ed..dc09705342b9bd2c8792f69b2c919adac328d57d 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Robot;
 import frc.robot.commands.gpm.AutoShootCommand;
 import frc.robot.commands.gpm.ClimbDriveCommand;
+import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
@@ -108,21 +109,25 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                     intake.spinStop();
                     intakeBoolean = true;
                 }
-            }));
+            }, intake));
 
             // Retract if hold for 3 seconds
             controller.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(() -> {
                 intake.retract();
                 intakeBoolean = true;
                 intake.spinStop();
-            }));
+            }, intake));
+
+            // Make the intake go in and out while shooter
+            controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
+                .alongWith(new InstantCommand(()-> intakeBoolean = true)));
 
             // Calibration
             controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
                 intake.calibrate();
             })).onFalse(new InstantCommand(() -> {
                 intake.stopCalibrating();
-            }));
+            }, intake));
 
             // Stop intake roller
             controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 75eaeca717eef87bc1d74fe23dcc742fb37e1406..3afbfab03db0c22c05a15fdc1d318f74d0900e2a 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -130,8 +130,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                //Sets the initial motor position
                motor.setPosition(motorRotations);
 
-               motor.setPosition(0.0);
-
                SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
                SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));
                SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);}));