diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ee32cba..baf67d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,11 +20,14 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; -import frc.robot.commands.IntakeCommand; +import frc.robot.commands.DiskIntakeCommand; +import frc.robot.commands.ExtendIntakeCommand; +import frc.robot.commands.RetractIntakeCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -115,7 +118,11 @@ private void configureBindings() { new Trigger(() -> { return m_driverController.getRightTriggerAxis() > 0.5; - }).whileTrue(new IntakeCommand(m_intakeSubsystem)); + }).whileTrue( + new SequentialCommandGroup( + new ExtendIntakeCommand(m_intakeSubsystem), + new DiskIntakeCommand(m_intakeSubsystem), + new RetractIntakeCommand(m_intakeSubsystem))); } /** diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/DiskIntakeCommand.java similarity index 68% rename from src/main/java/frc/robot/commands/IntakeCommand.java rename to src/main/java/frc/robot/commands/DiskIntakeCommand.java index fb9d504..a092513 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/DiskIntakeCommand.java @@ -4,14 +4,19 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; -public class IntakeCommand extends Command { +public class DiskIntakeCommand extends Command { private IntakeSubsystem m_intakeSubsystem; + boolean haveDisk = false; + + private Timer m_timer = new Timer(); + /** Creates a new intakeCommand. */ - public IntakeCommand(IntakeSubsystem subsystem) { + public DiskIntakeCommand(IntakeSubsystem subsystem) { m_intakeSubsystem = subsystem; addRequirements(m_intakeSubsystem); } @@ -19,25 +24,28 @@ public IntakeCommand(IntakeSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - m_intakeSubsystem.armExtend(); m_intakeSubsystem.intake(); + m_timer.reset(); + m_timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + if (m_intakeSubsystem.getDistanceSensor() < 6){ + haveDisk = true; + } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_intakeSubsystem.armRetract(); m_intakeSubsystem.stopIntake(); } + // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return haveDisk || m_timer.get() > 15; } } diff --git a/src/main/java/frc/robot/commands/ExtendIntakeCommand.java b/src/main/java/frc/robot/commands/ExtendIntakeCommand.java new file mode 100644 index 0000000..6ca4123 --- /dev/null +++ b/src/main/java/frc/robot/commands/ExtendIntakeCommand.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntakeSubsystem; + +public class ExtendIntakeCommand extends Command { + private IntakeSubsystem m_intakeSubsystem; + + /** Creates a new intakeCommand. */ + public ExtendIntakeCommand(IntakeSubsystem subsystem) { + m_intakeSubsystem = subsystem; + addRequirements(m_intakeSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_intakeSubsystem.armExtend(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_intakeSubsystem.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/commands/RetractIntakeCommand.java b/src/main/java/frc/robot/commands/RetractIntakeCommand.java new file mode 100644 index 0000000..4db1ec9 --- /dev/null +++ b/src/main/java/frc/robot/commands/RetractIntakeCommand.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntakeSubsystem; + +public class RetractIntakeCommand extends Command { + private IntakeSubsystem m_intakeSubsystem; + + /** Creates a new intakeCommand. */ + public RetractIntakeCommand(IntakeSubsystem subsystem) { + m_intakeSubsystem = subsystem; + addRequirements(m_intakeSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_intakeSubsystem.armRetract(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_intakeSubsystem.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ff3a4df..b5e73b9 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -14,10 +14,10 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.IntakeConstants; public class IntakeSubsystem extends SubsystemBase { - private boolean deployed = false; private boolean haveNote = false; private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); @@ -51,28 +51,24 @@ public IntakeSubsystem() { public void armExtend() { m_armPID.setSetpoint(IntakeConstants.kIntakeLoweredAngle); - - stopIntake(); - - deployed = true; } public void armRetract() { m_armPID.setSetpoint(IntakeConstants.kIntakeRaisedAngle); + } - stopIntake(); - - deployed = false; + public boolean atSetpoint(){ + return m_armPID.atSetpoint(); } public void intake() { - if (deployed && !haveNote) { + if (!haveNote) { m_intakeSpeed = IntakeConstants.kIntakeSpeed; } } public void outake() { - if (!deployed && haveNote) { + if (haveNote) { m_intakeSpeed = -IntakeConstants.kIntakeSpeed; } } @@ -91,24 +87,19 @@ public double getDistanceSensor() { @Override public void periodic() { - haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; - - // If we have a note and the arm is deployed, automatically bring it back in - if (haveNote && deployed) { - stopIntake(); - armRetract(); - } + haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold ? true : false; // m_armMotor.set(m_armPID.calculate(m_armEncoder.getDistance())); // m_intakeMotor.set(m_intakeSpeed); SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); - SmartDashboard.putBoolean("Arm Deployed?", deployed); SmartDashboard.putBoolean("Have Note?", haveNote); SmartDashboard.putNumber("pid output", m_armPID.calculate(m_armEncoder.getDistance())); } public boolean readyToShoot() { - return haveNote && !deployed && m_armPID.atSetpoint(); + return haveNote && + m_armPID.getSetpoint() == IntakeConstants.kIntakeRaisedAngle && + m_armPID.atSetpoint(); } }