From 91343469bde3e24b0073e8b4cb221da29371b588 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 10 Feb 2024 13:11:30 -0700 Subject: [PATCH] intake stuff works, still need to create command for handoff (im geekin) --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 59 ++++++++++++------- .../java/frc4388/robot/subsystems/Intake.java | 19 +++++- .../frc4388/robot/subsystems/Shooter.java | 1 + 4 files changed, 58 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 538bf93..1f3e0d8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -161,7 +161,8 @@ public final class Constants { public static final class IntakeConstants { 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.2; //TODO: + public static final double INTAKE_SPEED = 0.75; //TODO: + public static final double INTAKE_OUT_SPEED = 0.2; public static final double HANDOFF_SPEED = 0.2; //TODO: public static final double PIVOT_SPEED = 0.2; //TODO: diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5807995..ef68818 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -59,7 +59,9 @@ public class RobotContainer { - // private PlaybackChooser playbackChooser; + private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) + .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + .buildDisplay(); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -78,10 +80,8 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - // playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - // .addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - // .buildDisplay(); - //SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); + + SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); // m_robotIntake.resetPostion(); } @@ -97,6 +97,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + + /* Auto Recording */ // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, @@ -122,29 +124,42 @@ public class RobotContainer { /* Operator Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) - .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake)) // .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter)); - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors())); + // .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmIn())) + // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + + // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + // .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)); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake)) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); + + 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 f1dbb0f..d0de460 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -25,6 +25,12 @@ public class Intake extends SubsystemBase { private static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; private SparkLimitSwitch forwardLimit; private SparkLimitSwitch reverseLimit; + private SparkLimitSwitch intakeforwardLimit; + private SparkLimitSwitch intakereverseLimit; + + private Shooter shooter; + + /** Creates a new Intake. */ public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { @@ -38,7 +44,14 @@ public class Intake extends SubsystemBase { reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); forwardLimit.enableLimitSwitch(true); reverseLimit.enableLimitSwitch(true); + + intakeMotor.restoreFactoryDefaults(); + + intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeforwardLimit.enableLimitSwitch(true); + intakereverseLimit.enableLimitSwitch(false); //Arm PID m_spedController = pivot.getPIDController(); @@ -70,13 +83,17 @@ public class Intake extends SubsystemBase { } public void pidOut() { + m_spedController.setReference(-8000, CANSparkMax.ControlType.kVelocity); } + public void handoff() { - intakeMotor.set(-IntakeConstants.INTAKE_SPEED); + intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED); } + + public void stopIntakeMotors() { intakeMotor.set(0); } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index e30b9b2..e414aaf 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -43,5 +43,6 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + } }