From bc14a85fc794eae83b721dd8651fe6c885cbd146 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 15:16:21 -0600 Subject: [PATCH] extender intake group command DONE --- src/main/java/frc4388/robot/Constants.java | 6 +++ .../java/frc4388/robot/RobotContainer.java | 43 +++++++++++----- .../robot/commands/ExtenderIntakeGroup.java | 29 +++++++++++ .../frc4388/robot/commands/RunExtender.java | 49 +++++++++++++++++++ ...erOut.java => RunIntakeConditionally.java} | 24 ++++----- .../frc4388/robot/subsystems/Extender.java | 34 +++++++++---- .../java/frc4388/robot/subsystems/Intake.java | 6 ++- 7 files changed, 156 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java create mode 100644 src/main/java/frc4388/robot/commands/RunExtender.java rename src/main/java/frc4388/robot/commands/{RunExtenderOut.java => RunIntakeConditionally.java} (69%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a3599b..d54fce3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -146,6 +146,12 @@ public final class Constants { public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( false, 15, 0, 0); } + + public static final class ExtenderConstants { + public static final double EXTENDER_FORWARD_LIMIT = 250.0; + public static final double EXTENDER_REVERSE_LIMIT = 0.0; + } + public static final class StorageConstants { public static final int STORAGE_CAN_ID = 18; public static final int BEAM_SENSOR_SHOOTER = 28; //TODO diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 76b5d5b..0e0c200 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -54,6 +54,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.NotifierCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -61,6 +62,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.ExtenderIntakeGroup; import frc4388.robot.commands.RunMiddleSwitch; import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; @@ -237,8 +239,8 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); @@ -260,9 +262,9 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) @@ -272,14 +274,26 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new RunExtender(m_robotIntake, m_robotExtender, 0.5)) + // .whenReleased(new RunCommand(() -> RunExtender.changeDirection(), m_robotIntake, m_robotExtender)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection())); + + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new ParallelCommandGroup( + // new RunCommand(() -> m_robotIntake.m_intakeMotor.set(ExtenderIntakeGroup.direction * -0.2), m_robotIntake), + // new RunCommand(() -> m_robotExtender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 0.5), m_robotExtender))) + // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection())); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0))); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.switchDirection(), m_robotExtender)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) @@ -314,10 +328,15 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)); + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0), m_robotExtender)); // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java new file mode 100644 index 0000000..88d1cff --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java @@ -0,0 +1,29 @@ +// 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ExtenderIntakeGroup extends ParallelRaceGroup { + + public static int direction = 1; // assume extender starts retracted completely + + /** Creates a new RunExtenderAndIntake. */ + public ExtenderIntakeGroup(Intake intake, Extender extender) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + ExtenderIntakeGroup.direction = 1; + addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); + } + + public static void changeDirection() { + ExtenderIntakeGroup.direction *= -1; + } +} diff --git a/src/main/java/frc4388/robot/commands/RunExtender.java b/src/main/java/frc4388/robot/commands/RunExtender.java new file mode 100644 index 0000000..8af3e8f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtender.java @@ -0,0 +1,49 @@ +// 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ExtenderConstants; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +public class RunExtender extends CommandBase { + + private Extender extender; + + /** Creates a new RunExtender. */ + public RunExtender(Extender extender) { + // Use addRequirements() here to declare subsystem dependencies. + + this.extender = extender; + + addRequirements(this.extender); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + this.extender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 1.0); // TODO: change to 1.0 for actual speed, 0.5 is just for testing + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + ExtenderIntakeGroup.changeDirection(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (Math.abs(this.extender.m_extenderMotor.getEncoder().getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT) < 5) { + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOut.java b/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java similarity index 69% rename from src/main/java/frc4388/robot/commands/RunExtenderOut.java rename to src/main/java/frc4388/robot/commands/RunIntakeConditionally.java index b287baf..01f92b0 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOut.java +++ b/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java @@ -5,25 +5,19 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Intake; -public class RunExtenderOut extends CommandBase { +public class RunIntakeConditionally extends CommandBase { private Intake intake; - private Extender extender; - private int direction; - /** Creates a new RunExtenderOut. */ - public RunExtenderOut(Intake intake, Extender extender) { + /** Creates a new RunIntakeConditionally. */ + public RunIntakeConditionally(Intake intake) { // Use addRequirements() here to declare subsystem dependencies. - + this.intake = intake; - this.extender = extender; - this.direction = 1; - - addRequirements(this.intake, this.extender); + addRequirements(this.intake); } // Called when the command is initially scheduled. @@ -32,7 +26,13 @@ public class RunExtenderOut extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (ExtenderIntakeGroup.direction > 0) { + this.intake.m_intakeMotor.set(-0.4); + } else { + this.intake.m_intakeMotor.set(0); + } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 4be18e9..501bb56 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -4,20 +4,24 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ExtenderConstants; +import frc4388.utility.DesmosServer; public class Extender extends SubsystemBase { - CANSparkMax m_extenderMotor; + public CANSparkMax m_extenderMotor; private SparkMaxLimitSwitch m_inLimit; private SparkMaxLimitSwitch m_outLimit; public boolean toggle; - public int direction = 1; /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { @@ -26,13 +30,28 @@ public class Extender extends SubsystemBase { m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(true); - m_outLimit.enableLimitSwitch(true); + m_inLimit.enableLimitSwitch(false); + m_outLimit.enableLimitSwitch(false); + + m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT); + m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT); + setExtenderSoftLimits(true); + } + + /** + * Set status of extender motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setExtenderSoftLimits(boolean set) { + m_extenderMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); + DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition()); } /** @@ -47,12 +66,7 @@ public class Extender extends SubsystemBase { public void runExtender(double input) { // if (!m_serializer.getBeam() && input < 0.) return; - if (this.direction > 0) {} - m_extenderMotor.set(this.direction * input); - } - - public void switchDirection() { - this.direction = this.direction * -1; + m_extenderMotor.set(input); } public double getCurrent() { diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index cb4a73d..3ad0491 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -12,11 +12,13 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.commands.ExtenderIntakeGroup; + import com.revrobotics.CANSparkMax; public class Intake extends SubsystemBase { - private WPI_TalonFX m_intakeMotor; + public WPI_TalonFX m_intakeMotor; private Serializer m_serializer; /** Creates a new Intake. */ @@ -28,6 +30,8 @@ public class Intake extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake Percent Output", m_intakeMotor.get()); + SmartDashboard.putNumber("Extender Direction", ExtenderIntakeGroup.direction); } /** * Runs The Intake With Triggers as input