added driveforwardcommand

grt192 · Feb 10, 2024 · 0ea69fe · 0ea69fe
1 parent c5685f4
commit 0ea69fe
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/commands/auton/BaseAutonSequence.java b/src/main/java/frc/robot/commands/auton/BaseAutonSequence.java
@@ -64,14 +64,10 @@ public Command followPath(ChoreoTrajectory traj){
         return swerveCommand;
     }
 
-    public ParallelDeadlineGroup driveForward(double xpower, double ypower, double angularpower){
-
-        return new ParallelDeadlineGroup( new IntakeRollerIntakeCommand(intakeRollersSubsystem));
-    }
-    public SequentialCommandGroup goIntake(ChoreoTrajectory intaketraj, double xpower, double ypower, double angularpower ){
+    public SequentialCommandGroup goIntake(ChoreoTrajectory intaketraj){
         return followPath(intaketraj)
         .andThen(new IntakePivotExtendedCommand(intakePivotSubsystem))
-        .andThen(driveForward(xpower, ypower, angularpower))
+        .andThen(new IntakeRollerIntakeCommand(intakeRollersSubsystem).deadlineWith(new DriveForwardCommand(swerveSubsystem).withTimeout(driveforwardtime)))
         .andThen(new IntakeRollerIntakeCommand(intakeRollersSubsystem))//just in case the note isn't fully intaken during intakeandMove 
         .andThen(new IntakePivotVerticalCommand(intakePivotSubsystem));
     }

diff --git a/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java b/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java
@@ -5,9 +5,22 @@
 
 public class DriveForwardCommand extends Command{
     private final SwerveSubsystem swerve;
+    private int xpower;
+    private int ypower;
 
     public DriveForwardCommand(SwerveSubsystem swerve){
         this.swerve = swerve;
         addRequirements(swerve);
     }
+
+    @Override
+    public void initialize() {
+        swerve.setDrivePowers(xpower, ypower, 0);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerve.setDrivePowers(0,0,0);
+    }
+
 }