From 0e063176cd270f11f6be2e9af5c52c22d32a27c8 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 5 Mar 2022 22:57:55 -0700 Subject: [PATCH] Intake fixes --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 52 +++++++++++++++---- .../java/frc4388/robot/subsystems/Intake.java | 13 +++-- .../frc4388/robot/subsystems/Serializer.java | 40 ++++---------- .../frc4388/robot/subsystems/Storage.java | 9 ++-- 5 files changed, 65 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8b0c190..3945aed 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -135,6 +135,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 17; public static final int BEAM_SENSOR_SHOOTER = 28; //TODO public static final int BEAM_SENSOR_INTAKE = 29; //TODO + public static final double STORAGE_SPEED = 0.3; } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b40b252..b7b81df 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,6 +56,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; @@ -88,14 +89,15 @@ public class RobotContainer { // Subsystems public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor); private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom); + /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -131,6 +133,7 @@ public class RobotContainer { // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, // m_robotSwerveDrive)); + //Swerve Drive m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), @@ -139,6 +142,20 @@ public class RobotContainer { getDriverController().getRightY(), true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + //Intake with Triggers + m_robotIntake.setDefaultCommand( + new RunCommand(() -> m_robotIntake.runWithTriggers( + getOperatorController().getLeftTriggerAxis(), + getOperatorController().getRightTriggerAxis()), + m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + //Storage Management + m_robotStorage.setDefaultCommand( + new RunCommand(() -> m_robotStorage.manageStorage(), + m_robotStorage).withName("Storage manageStorage defaultCommand")); + //Serializer Management + m_robotSerializer.setDefaultCommand( + new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), + m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on /* @@ -157,9 +174,10 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + /* Driver Buttons */ // "XboxController.Button.kBack" was undefined yet, 7 works just fine - new JoystickButton(getDriverController(), 7) + new JoystickButton(getDriverController(), XboxController.Button.kBack.value) .whenPressed(m_robotSwerveDrive::resetGyro); new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) @@ -174,27 +192,39 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - new JoystickButton(getDriverController(), XboxController.Button.kX.value) + + new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp .whenPressed(() -> m_robotMap.leftFront.reset()) .whenPressed(() -> m_robotMap.rightFront.reset()) .whenPressed(() -> m_robotMap.leftBack.reset()) .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ - // activates "Lit Mode" /* - * new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - * - * new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - * .whenPressed(new InstantCommand()); - * * // activates "BoomBoom" * new JoystickButton(getOperatorController(), XboxController.Button.kA.value) * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, * m_robotHood)); */ + + //Extender + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(() -> m_robotIntake.runExtender(true)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(() -> m_robotIntake.runExtender(false)); + + + + //Storage + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) + .whenReleased(() -> m_robotStorage.runStorage(0.0)); + + + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) + .whenReleased(() -> m_robotStorage.runStorage(0.0)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 56c80b8..0c63afc 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -20,15 +20,17 @@ public class Intake extends SubsystemBase { private WPI_TalonFX m_intakeMotor; private CANSparkMax m_extenderMotor; + private Serializer m_serializer; private SparkMaxLimitSwitch m_inLimit; private SparkMaxLimitSwitch m_outLimit; public boolean toggle; /** Creates a new Intake. */ - public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor) { + public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) { m_intakeMotor = intakeMotor; m_extenderMotor = extenderMotor; + m_serializer = serializer; m_extenderMotor.restoreFactoryDefaults(); @@ -59,9 +61,15 @@ public class Intake extends SubsystemBase { * @param extended Wether the Extender Is Extended */ public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?) - double extenderMotorSpeed = extended ? 0.25d : 0.d; + if (!m_serializer.getBeam() && !extended) return; + double extenderMotorSpeed = extended ? 0.25d : -0.25d; m_extenderMotor.set(extenderMotorSpeed); } + + public void runExtender(double input) { + if (!m_serializer.getBeam() && input < 0.) return; + m_extenderMotor.set(input); + } /** * Toggles The Extender */ @@ -69,5 +77,4 @@ public class Intake extends SubsystemBase { toggle = !toggle; runExtender(toggle); } - //Test } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index 1309e01..5263fb8 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; +import frc4388.robot.Constants.SerializerConstants; import edu.wpi.first.wpilibj.DigitalInput; import com.revrobotics.CANSparkMax; @@ -14,15 +15,16 @@ public class Serializer extends SubsystemBase{ public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { m_serializerBelt = serializerBelt; - // m_serializerShooterBelt = serializerShooterBelt; m_serializerBeam = serializerBeam; - serializerState = false; - setSerializerState(serializerState); m_serializerBelt.set(0); // m_serializerShooterBelt.set(0); } + + public void setSerializer(double input){ + m_serializerBelt.set(input); + } /** * Gets The State Of The Beam * @return The State Of The Beam @@ -30,40 +32,16 @@ public class Serializer extends SubsystemBase{ public boolean getBeam() { return m_serializerBeam.get(); } + /** * Sets The Serializer State With The Beam * @param state Your State Of The Button * @param beambroken The State of the Beam Senser */ - public void setSerializerStateWithBeam(boolean state, boolean beambroken) { - boolean total = state || beambroken; - setSerializerState(total); + public void setSerializerStateWithBeam() { + if (m_serializerBeam.get()) setSerializer(0.0); + else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED); } - /** - * Sets The Serializer State With The Beam - * @param state Your State Of The Button - */ - public void setSerializerState(boolean state) { - setSerializerBeltState(state); - // setSerializerShooterBeltState(state); - serializerState = state; - } - /** - * Sets the Serializer Belt State - * @param state Your State Of The Button - */ - public void setSerializerBeltState(boolean state) { - double serializerBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_BELT_SPEED : 0.d; - m_serializerBelt.set(serializerBeltSpeed); - } - // /** - // * Sets the Shooter Belt State - // * @param state Your State Of The Button - // */ - // public void setSerializerShooterBeltState(boolean state) { - // double serializerShooterBeltSpeed = state ? Constants.SerializerConstants.SERIALIZER_SHOOTER_BELT_SPEED : 0.d; - // m_serializerShooterBelt.set(serializerShooterBeltSpeed); - // } /** * Gets the Serializer State diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7b0a4d7..1e3126a 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -26,9 +26,8 @@ public class Storage extends SubsystemBase { * If Else, Stop Running Storage */ public void manageStorage() { - if (getBeamIntake()) { //Maybe needs to be shooter - runStorage(1.d); - } else { runStorage(0.d); } + if (getBeamIntake()) runStorage(0.d); + else runStorage(1.d); } /** @@ -43,7 +42,7 @@ public class Storage extends SubsystemBase { * @return The State Of The Beam on the Shooter */ public boolean getBeamShooter(){ - return m_beamShooter.get(); + return m_beamShooter.get();//True if open } /** @@ -51,7 +50,7 @@ public class Storage extends SubsystemBase { * @return The Beam State Of The Intake */ public boolean getBeamIntake(){ - return m_beamIntake.get(); + return m_beamIntake.get(); //True if open }