feat: Timed Commands for auton

SaintsRobotics · Feb 24, 2024 · dd7bca8 · dd7bca8
1 parent 55b7e33
commit dd7bca8
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -111,13 +111,15 @@ public static class IntakeConstants {
         public static final double kArmI = 0;
         public static final double kArmD = 0;
         public static final int kArmEncoderCh = 0;
+        public static final double kTimeIntake = 5;
         public static double kIntakeSpeed;
     }
 
     // Shooter subsystem speed constants
     public static class ShooterConstants {
         public static final double kSpinSpeedTrue = 0.75;
         public static final double kSpinSpeedFalse = 0;
+        public static final double kTimeShoot = 5;
         public static int kBottomShooterMotorPort;
         public static int kTopShooterMotorPort;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -50,9 +50,9 @@ public RobotContainer() {
 
     //Registering Named Commands for autonpaths
     //registerCommand is looking for XXXXXXCommand, so things may need to be renamed. 
-   NamedCommands.registerCommand("shoot", m_shooterSubsystem.spin(0.75));
-   NamedCommands.registerCommand("align", DriverSubsystem.autoAlign());
-   NamedCommands.registerCommand("intake", m_intakeSubsystem.intakeDisk());
+   NamedCommands.registerCommand("shoot", new InstantCommand(() -> m_shooterSubsystem.shooterTimedRun(Constants.ShooterConstants.kTimeShoot, Constants.ShooterConstants.kSpinSpeedTrue)));
+   //NamedCommands.registerCommand("align", DriverSubsystem.autoAlign());
+   NamedCommands.registerCommand("intake", new InstantCommand(() -> m_intakeSubsystem.intakeTimedRun(Constants.IntakeConstants.kTimeIntake), m_intakeSubsystem));
 
     //All paths automatically 
     autoChooser = AutoBuilder.buildAutoChooser();

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -9,6 +9,9 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.IntakeConstants;
@@ -39,6 +42,17 @@ public void stopIntaking() {
     m_intakeMotor.set(0);
   }
 
+  public void intakeTimedRun(double time){
+    final Timer timer = new Timer();
+    timer.reset();
+    timer.start();
+    while(timer.get() != time){
+      m_intakeMotor.set(IntakeConstants.kIntakeSpeed);
+    }
+    m_intakeMotor.set(0);
+
+  }
+
   /**
    * Rotates the arm to a given angle
    * @param angle motor to apply to intake

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -7,7 +7,9 @@
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.ShooterConstants;
 
 public class ShooterSubsystem extends SubsystemBase {
@@ -24,6 +26,18 @@ public void spin(double speed) {
     m_top.set(speed);
   }
 
+  public void shooterTimedRun(double time, double speed){
+    final Timer timer = new Timer();
+    timer.reset();
+    timer.start();
+    while(timer.get() != time){
+      m_bottom.set(speed);
+      m_top.set(speed);
+    }
+    m_bottom.set(0);
+    m_top.set(0);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run