diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1f3e0d8..18ac32b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -162,12 +162,12 @@ public final class Constants { public static final int INTAKE_MOTOR_ID = 18; //TODO: public static final int PIVOT_MOTOR_ID = 17; //TODO: public static final double INTAKE_SPEED = 0.75; //TODO: - public static final double INTAKE_OUT_SPEED = 0.2; + public static final double INTAKE_OUT_SPEED = 0.5; public static final double HANDOFF_SPEED = 0.2; //TODO: public static final double PIVOT_SPEED = 0.2; //TODO: public static final class ArmPID { - public static final Gains INTAKE_GAINS = new Gains(0.00005, 0, 0, 0, 0, 1.0); + public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0); } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ef68818..29c86f8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,11 +7,16 @@ package frc4388.robot; +import java.time.Instant; + import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; 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.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.Autos.PlaybackChooser; @@ -57,6 +62,28 @@ public class RobotContainer { private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + + + private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.pidIn()), + new InstantCommand(() -> m_robotShooter.spin(0.4)) + ); + + private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin()), + new InstantCommand(() -> m_robotIntake.handoff()) + ); + + private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.rotateArmOut2()), + new RunCommand(() -> m_robotIntake.limitNote()).until(m_robotIntake.getArmFowardLimitState()), + new InstantCommand(() -> m_robotShooter.spin(0.4)) + ); + + + + + private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) @@ -140,25 +167,43 @@ public class RobotContainer { // .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmOut())) // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake)) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); + // //Pull arm in + // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake)) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); + + // //Pull arm out + // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake)) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); + + // //Intake Note + // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); + + // //Outtake Note + // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); + + // //Spin Shooter Motors + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) + // .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); + + // //Intake Note and ramp up shooter to 40% + // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + // .onTrue(intakeToShoot); + + // //Ramps up shooter to 100% to Shooter + // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + // .onTrue(outtakeToShootFull); + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake)) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); + .whileTrue(intakeInToOut); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) - .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_TRIGGER_AXIS) - .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)) - .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) - .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index d0de460..aa3c43c 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import java.util.function.BooleanSupplier; + import com.revrobotics.CANSparkMax; import com.revrobotics.SparkLimitSwitch; import com.revrobotics.SparkPIDController; @@ -78,13 +80,36 @@ public class Intake extends SubsystemBase { } public void pidIn() { - m_spedController.setReference(8000, CANSparkMax.ControlType.kVelocity); + m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); } - public void pidOut() { - m_spedController.setReference(-8000, CANSparkMax.ControlType.kVelocity); + public void limitNote() { + if (intakeforwardLimit.isPressed()) { + rotateArmIn2(); + } else { + spinIntakeMotor(); + } + } + public void pidOut() { + m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); + } + + public void rotateArmOut2() { + if(reverseLimit.isPressed()){ + stopArmMotor(); + } else { + pidOut(); + } + } + + public void rotateArmIn2() { + if(forwardLimit.isPressed()){ + stopArmMotor(); + } else { + pidIn(); + } } @@ -131,6 +156,10 @@ public class Intake extends SubsystemBase { } + public BooleanSupplier getArmFowardLimitState() { + return forwardLimit::isPressed; + } + @Override public void periodic() { // This method will be called once per scheduler run