diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a963b8d..45c14d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,6 +111,7 @@ 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; } @@ -118,6 +119,7 @@ public static class IntakeConstants { 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 index 316fa95..485841b 100644 --- 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 index 9d55b9c..f715ea5 100644 --- 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 index a47de12..f75b4aa 100644 --- 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