From 8a902e635f592cb2bcb83c3d7c85c8a52333a93b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 20:10:12 -0600 Subject: [PATCH] extender and intake changes --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 47 ++++++++----------- .../ExtenderIntakeGroup.java | 2 +- .../RunExtender.java | 31 +++++++++--- .../RunIntakeConditionally.java | 7 +-- .../{ => ShooterCommands}/AimToCenter.java | 2 +- .../commands/{ => ShooterCommands}/Shoot.java | 2 +- .../{ => ShooterCommands}/TrackTarget.java | 2 +- .../frc4388/robot/subsystems/Extender.java | 30 ++++-------- .../java/frc4388/robot/subsystems/Intake.java | 25 +++++----- .../frc4388/commands/AimToCenterTest.java | 3 +- 11 files changed, 77 insertions(+), 75 deletions(-) rename src/main/java/frc4388/robot/commands/{ => ExtenderIntakeCommands}/ExtenderIntakeGroup.java (95%) rename src/main/java/frc4388/robot/commands/{ => ExtenderIntakeCommands}/RunExtender.java (62%) rename src/main/java/frc4388/robot/commands/{ => ExtenderIntakeCommands}/RunIntakeConditionally.java (86%) rename src/main/java/frc4388/robot/commands/{ => ShooterCommands}/AimToCenter.java (98%) rename src/main/java/frc4388/robot/commands/{ => ShooterCommands}/Shoot.java (99%) rename src/main/java/frc4388/robot/commands/{ => ShooterCommands}/TrackTarget.java (98%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c57bd9d..c20669b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -145,6 +145,7 @@ public final class Constants { false, 10, 0, 0); //Find public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( false, 15, 0, 0); + public static final double INTAKE_SPEED_MULTIPLIER = 0.4; } public static final class ExtenderConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e64241f..ffcc52b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,11 +62,11 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; 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; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; +import frc4388.robot.commands.ShooterCommands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Extender; @@ -96,13 +96,14 @@ import frc4388.utility.controller.DeadbandedXboxController; */ public class RobotContainer { private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); - /* RobotMap */ + + // RobotMap public final RobotMap m_robotMap = new RobotMap(); // Subsystems public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotSerializer); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); @@ -115,12 +116,12 @@ public class RobotContainer { private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); public final Climber m_robotClimber = new Climber(testElbowMotor); - /* Controllers */ + // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); - /* Autonomous */ + // Autonomous private PathPlannerTrajectory loadedPathTrajectory = null; private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List pathPoints = new ArrayList<>(); @@ -275,20 +276,8 @@ 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))); @@ -297,10 +286,14 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0))); + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(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)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); @@ -321,8 +314,8 @@ public class RobotContainer { /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) @@ -343,7 +336,7 @@ public class RobotContainer { .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_robotExtender.m_extenderMotor.getEncoder().setPosition(0), m_robotExtender)); + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(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/ExtenderIntakeCommands/ExtenderIntakeGroup.java similarity index 95% rename from src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java rename to src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 88d1cff..2e9748f 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ExtenderIntakeCommands; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import frc4388.robot.subsystems.Extender; diff --git a/src/main/java/frc4388/robot/commands/RunExtender.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java similarity index 62% rename from src/main/java/frc4388/robot/commands/RunExtender.java rename to src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java index 8af3e8f..4f4183e 100644 --- a/src/main/java/frc4388/robot/commands/RunExtender.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ExtenderIntakeCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ExtenderConstants; @@ -13,23 +13,38 @@ public class RunExtender extends CommandBase { private Extender extender; + private double error; + private double tolerance; + /** Creates a new RunExtender. */ public RunExtender(Extender extender) { // Use addRequirements() here to declare subsystem dependencies. - this.extender = extender; - + + updateError(); + tolerance = 5.0; + addRequirements(this.extender); } - + + public void updateError() { + if (ExtenderIntakeGroup.direction > 0) { + this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT); + } else { + this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_REVERSE_LIMIT); + } + } + // 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 + System.out.println("RunExtender is working"); + this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0); + updateError(); } // Called once the command ends or is interrupted. @@ -41,7 +56,9 @@ public class RunExtender extends CommandBase { // 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) { + if (error < tolerance) { + System.out.println("RunExtender finished"); + this.extender.runExtender(0.0); return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java similarity index 86% rename from src/main/java/frc4388/robot/commands/RunIntakeConditionally.java rename to src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java index 01f92b0..9584967 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java @@ -2,9 +2,10 @@ // 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; +package frc4388.robot.commands.ExtenderIntakeCommands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.Intake; public class RunIntakeConditionally extends CommandBase { @@ -28,9 +29,9 @@ public class RunIntakeConditionally extends CommandBase { @Override public void execute() { if (ExtenderIntakeGroup.direction > 0) { - this.intake.m_intakeMotor.set(-0.4); + this.intake.runAtOutput(-1); } else { - this.intake.m_intakeMotor.set(0); + this.intake.runAtOutput(0); } } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java similarity index 98% rename from src/main/java/frc4388/robot/commands/AimToCenter.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 9523577..7f8a8db 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java similarity index 99% rename from src/main/java/frc4388/robot/commands/Shoot.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index b2a574f..94bb04f 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java similarity index 98% rename from src/main/java/frc4388/robot/commands/TrackTarget.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 8a5f59f..04d607c 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import java.util.ArrayList; diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 1680b88..34cd555 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -15,13 +15,11 @@ import frc4388.robot.Constants.ExtenderConstants; public class Extender extends SubsystemBase { - public CANSparkMax m_extenderMotor; + private CANSparkMax m_extenderMotor; private SparkMaxLimitSwitch m_inLimit; private SparkMaxLimitSwitch m_outLimit; - public boolean toggle; - /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { @@ -52,30 +50,20 @@ public class Extender extends SubsystemBase { SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); } - /** - * Runs The Extender- - * @param extended Wether the Extender Is Extended - */ - // public void runExtender(boolean extended) { - // 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); } + public double getPosition() { + return m_extenderMotor.getEncoder().getPosition(); + } + + public void setEncoder(double position) { + m_extenderMotor.getEncoder().setPosition(position); + } + public double getCurrent() { return m_extenderMotor.getOutputCurrent(); } - - /** - * Toggles The Extender - */ - // public void toggleExtender() { - // toggle = !toggle; - // runExtender(toggle); - // } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 3ad0491..a98bc05 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,27 +4,20 @@ package frc4388.robot.subsystems; -//Imported Limit switch ONLY -import com.revrobotics.SparkMaxLimitSwitch; - -import com.ctre.phoenix.motorcontrol.NeutralMode; 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; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; public class Intake extends SubsystemBase { - public WPI_TalonFX m_intakeMotor; - private Serializer m_serializer; + private WPI_TalonFX m_intakeMotor; /** Creates a new Intake. */ - public Intake(WPI_TalonFX intakeMotor, Serializer serializer) { + public Intake(WPI_TalonFX intakeMotor) { m_intakeMotor = intakeMotor; - m_serializer = serializer; } @Override @@ -39,10 +32,18 @@ public class Intake extends SubsystemBase { * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { - m_intakeMotor.set((rightTrigger - leftTrigger) * 0.4); + m_intakeMotor.set((rightTrigger - leftTrigger) * IntakeConstants.INTAKE_SPEED_MULTIPLIER); SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } + + public void runAtOutput(double output, double multiplier) { + m_intakeMotor.set(output * multiplier); + } + + public void runAtOutput(double output) { + m_intakeMotor.set(output * IntakeConstants.INTAKE_SPEED_MULTIPLIER); + } public double getCurrent() { return m_intakeMotor.getSupplyCurrent(); diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 644d3a9..0bc247d 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -2,7 +2,8 @@ package frc4388.commands; import org.junit.Test; -import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.AimToCenter; + import org.junit.Assert; public class AimToCenterTest {