From 79b75bd99018e720d73d7e9fc36bfe6ae4c93ef5 Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 09:53:08 -0400 Subject: [PATCH 01/22] Double, double, toil and trouble --- src/main/java/frc/robot/RobotContainer.java | 7 +++- .../commands/auto/TwoRingAutoCommand.java | 41 +++++++++++++++++++ 2 files changed, 46 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 320962d..efdcbaa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.commands.auto.ShootAndDriveAutoCommand; import frc.robot.commands.auto.ShootAutoCommand; import frc.robot.commands.auto.TurnAndShootAutoCommand; +import frc.robot.commands.auto.TwoRingAutoCommand; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -89,7 +90,7 @@ public RobotContainer() { m_delay = m_tab.add("Delay", 0).getEntry(); m_chosenAuto.setDefaultOption("Shoot and drive from middle", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //used to be an empty Pose2D (should not change but maybe the empty pose has something to do with it) + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0))); m_chosenAuto.addOption("Shoot and drive from left", new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0))); m_chosenAuto.addOption("Shoot and drive from right", @@ -99,9 +100,11 @@ public RobotContainer() { m_chosenAuto.addOption("Drive, turn, and shoot from right", new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0))); m_chosenAuto.addOption("Drive back only", - new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); //drive back only auto + new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); m_chosenAuto.addOption("Shoot only", new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0)).withTimeout(4)); + m_chosenAuto.addOption("2 rings", + new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0))); m_tab.add(m_chosenAuto); diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java new file mode 100644 index 0000000..9087be5 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -0,0 +1,41 @@ +// 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 frc.robot.commands.auto; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.GroundIntakeCommand; +import frc.robot.commands.SetPoseCommand; +import frc.robot.commands.ShootCommand; +import frc.robot.commands.SwerveDriveCommand; +import frc.robot.commands.TurnThetaCommand; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.SwerveSubsystem; + +public class TwoRingAutoCommand extends SequentialCommandGroup { + public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { + super( + new WaitCommand(delay), + new ShootCommand(shoot).withTimeout(1.5), + new SetPoseCommand(swerve, startingOffset).withTimeout(0), + new ParallelCommandGroup( + new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), + new GroundIntakeCommand(intake) + ).withTimeout(3), + new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) + .withTimeout(3.2), + new ShootCommand(shoot).withTimeout(1.5) + ); + } + + private static Supplier supplyDouble(double d) {return new Supplier() {@Override public Double get() {return d;}};} + private static Supplier supplyBoolean(boolean b) {return new Supplier() {@Override public Boolean get() {return b;}};} +} From acc8fa1f3db5c09846fb8479d0e5c776f467d872 Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 10:01:29 -0400 Subject: [PATCH 02/22] Rebind + no third wheel --- src/main/java/frc/robot/RobotContainer.java | 5 ++- .../robot/subsystems/ShooterSubsystem.java | 32 +++++++++---------- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index efdcbaa..2358e65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -137,8 +137,6 @@ private void configureButtonBindings() { .whileTrue(new ExpelRingCommand(m_intakeSubsystem)); new JoystickButton(m_driverController, XboxController.Button.kX.value) .whileTrue(new FeedShooterCommand(m_intakeSubsystem)); - new JoystickButton(m_driverController, XboxController.Button.kY.value) - .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); // Left bumper = Toggle fastmode // Left trigger = Toggle fastermode // POV = Nudge @@ -149,7 +147,8 @@ private void configureButtonBindings() { new JoystickButton(m_auxController, XboxController.Button.kRightBumper.value) // Shoot .whileTrue(new ShootCommand(m_shooterSubsystem)); new JoystickButton(m_auxController, XboxController.Button.kLeftBumper.value) // Intake - .whileTrue(new IntakeCommand(m_shooterSubsystem)); + .whileTrue(new IntakeCommand(m_shooterSubsystem)) + .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 16c6734..a01c58b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -25,14 +25,14 @@ public class ShooterSubsystem extends SubsystemBase { private final GenericEntry m_backMotorPower; private final GenericEntry m_frontIntakePower; private final GenericEntry m_backIntakePower; - private final GenericEntry m_indexPower; - private final GenericEntry m_indexIntakePower; + // private final GenericEntry m_indexPower; + // private final GenericEntry m_indexIntakePower; private final GenericEntry m_winchUpPower; private final GenericEntry m_winchDownPower; public final CANSparkFlex m_frontMotor; public final CANSparkFlex m_backMotor; - public final CANSparkMax m_indexMotor; + // public final CANSparkMax m_indexMotor; public final CANSparkMax m_winchMotor; public final RelativeEncoder m_winchEncoder; @@ -40,15 +40,15 @@ public ShooterSubsystem() { m_frontMotor = new CANSparkFlex(ShooterConstants.kFrontMotorCanId, MotorType.kBrushless); m_backMotor = new CANSparkFlex(ShooterConstants.kBackMotorCanId, MotorType.kBrushless); - m_indexMotor = new CANSparkMax(ShooterConstants.kIndexMotorCanId, MotorType.kBrushless); + // m_indexMotor = new CANSparkMax(ShooterConstants.kIndexMotorCanId, MotorType.kBrushless); m_winchMotor = new CANSparkMax(ShooterConstants.kWinchMotorCanId, MotorType.kBrushed); m_frontMotor.setIdleMode(IdleMode.kCoast); m_backMotor.setIdleMode(IdleMode.kCoast); - m_indexMotor.setIdleMode(IdleMode.kCoast); + // m_indexMotor.setIdleMode(IdleMode.kCoast); m_winchMotor.setIdleMode(IdleMode.kCoast); m_frontMotor.setInverted(false); m_backMotor.setInverted(false); - m_indexMotor.setInverted(true); + // m_indexMotor.setInverted(true); m_winchMotor.setInverted(false); m_winchEncoder = m_winchMotor.getEncoder(Type.kQuadrature, 8192); @@ -64,10 +64,10 @@ public ShooterSubsystem() { .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_backIntakePower = m_shuffleboardtab.add("Back Intake Power", ShooterConstants.kBackIntakePower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_indexPower = m_shuffleboardtab.add("Index Power", ShooterConstants.kIndexPower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_indexIntakePower = m_shuffleboardtab.add("Index Intake Power", ShooterConstants.kIndexIntakePower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + // m_indexPower = m_shuffleboardtab.add("Index Power", ShooterConstants.kIndexPower) + // .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + // m_indexIntakePower = m_shuffleboardtab.add("Index Intake Power", ShooterConstants.kIndexIntakePower) + // .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_winchUpPower = m_shuffleboardtab.add("Winch Up Power", ShooterConstants.kWinchUpPower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", ShooterConstants.kWinchDownPower) @@ -88,7 +88,7 @@ public double[] getIntakePowers() { return new double[] { clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)), clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)), - clampPower(m_indexIntakePower.getDouble(ShooterConstants.kIndexIntakePower)) + // clampPower(m_indexIntakePower.getDouble(ShooterConstants.kIndexIntakePower)) }; } public void runShooter() { @@ -102,12 +102,12 @@ public void runShooter(double rearPower) { public void runIntake() { m_frontMotor.set(getIntakePowers()[0]); m_backMotor.set(getIntakePowers()[1]); - m_indexMotor.set(getIntakePowers()[2]); + // m_indexMotor.set(getIntakePowers()[2]); } public void stopShooter() { m_frontMotor.stopMotor(); m_backMotor.stopMotor(); - m_indexMotor.stopMotor(); + // m_indexMotor.stopMotor(); } public boolean isShooterRunning() { if (m_frontMotor.get() == 0 && m_backMotor.get() == 0) { //Check if both motors are stopped @@ -118,9 +118,9 @@ public boolean isShooterRunning() { } // Indexer stuff - public double getIndexPower() {return clampPower(m_indexPower.getDouble(ShooterConstants.kIndexPower));} - public void runIndex() {m_indexMotor.set(getIndexPower());} - public void stopIndex() {m_indexMotor.stopMotor();} + // public double getIndexPower() {return clampPower(m_indexPower.getDouble(ShooterConstants.kIndexPower));} + // public void runIndex() {m_indexMotor.set(getIndexPower());} + // public void stopIndex() {m_indexMotor.stopMotor();} // Winch stuff public double[] getWinchPowers() { From fe3c6849e9633856e38c04a8520bd89ffd3f3588 Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 10:03:11 -0400 Subject: [PATCH 03/22] errrrr --- src/main/java/frc/robot/commands/ShooterCommand.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index ddfdb4d..62db8fb 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -23,19 +23,15 @@ public ShooterCommand(ShooterSubsystem subsystem) { @Override public void execute() { if (m_commandType.equals("both")) m_subsystem.runShooter(); - else if (m_commandType.equals("all")) { - m_subsystem.runShooter(); - m_subsystem.runIndex(); - } else if (m_commandType.equals("front")) m_subsystem.runShooter(0.0); - else if (m_commandType.equals("index")) m_subsystem.runIndex(); + // else if (m_commandType.equals("index")) m_subsystem.runIndex(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { if (m_commandType.equals("both") || m_commandType.equals("front") || m_commandType.equals("all")) m_subsystem.stopShooter(); - if (m_commandType.equals("index") || m_commandType.equals("all")) m_subsystem.stopIndex(); + // if (m_commandType.equals("index") || m_commandType.equals("all")) m_subsystem.stopIndex(); } // Returns true when the command should end. From 99d4126ff998400059571256be253a454aea1715 Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 10:07:44 -0400 Subject: [PATCH 04/22] Follow-through --- .../java/frc/robot/commands/auto/TwoRingAutoCommand.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java index 9087be5..5fcf24d 100644 --- a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -15,7 +15,6 @@ import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; -import frc.robot.commands.TurnThetaCommand; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; @@ -24,7 +23,7 @@ public class TwoRingAutoCommand extends SequentialCommandGroup { public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { super( new WaitCommand(delay), - new ShootCommand(shoot).withTimeout(1.5), + new ShootCommand(shoot).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new ParallelCommandGroup( new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), @@ -32,7 +31,7 @@ public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Intake ).withTimeout(3), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(3.2), - new ShootCommand(shoot).withTimeout(1.5) + new ShootCommand(shoot).withTimeout(4) ); } From e2726cbdf9109e11d7fc03868e620d50e2fd262d Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 10:08:49 -0400 Subject: [PATCH 05/22] xtras --- src/main/java/frc/robot/commands/auto/DriveAutoCommand.java | 3 --- .../java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java | 2 -- src/main/java/frc/robot/commands/auto/ShootAutoCommand.java | 2 -- .../java/frc/robot/commands/auto/SlideAndShootAutoCommand.java | 2 -- .../java/frc/robot/commands/auto/TurnAndShootAutoCommand.java | 2 -- src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java | 1 - 6 files changed, 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java index da73aff..cc0144d 100644 --- a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java @@ -7,12 +7,9 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; -import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.TurnThetaCommand; import frc.robot.subsystems.ShooterSubsystem; diff --git a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java index e5570cb..eb60d24 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java @@ -7,8 +7,6 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; diff --git a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java index af9b6d7..aa58917 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java @@ -5,8 +5,6 @@ package frc.robot.commands.auto; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; diff --git a/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java index cb3b664..e985134 100644 --- a/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java @@ -7,8 +7,6 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; diff --git a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java index 374938b..d6b67df 100644 --- a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java @@ -7,8 +7,6 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java index 5fcf24d..1c455fd 100644 --- a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.GroundIntakeCommand; From 43663913388f65cf1fabbb7a7c20193c315b09ce Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 10:09:26 -0400 Subject: [PATCH 06/22] more unused stuff --- src/main/java/frc/robot/commands/TurnThetaCommand.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/TurnThetaCommand.java b/src/main/java/frc/robot/commands/TurnThetaCommand.java index 3308af5..62e3667 100644 --- a/src/main/java/frc/robot/commands/TurnThetaCommand.java +++ b/src/main/java/frc/robot/commands/TurnThetaCommand.java @@ -22,7 +22,6 @@ public class TurnThetaCommand extends Command { public final Supplier ySupplier = ()-> 0.0; public final Supplier turnSupplier = ()-> 0.0; private boolean isTurnFinished = false; - private double initial_heading; private double targetTheta; public TurnThetaCommand(SwerveSubsystem subsystem, double turnDegrees) { @@ -42,7 +41,6 @@ public TurnThetaCommand(SwerveSubsystem subsystem, double turnDegrees) { @Override public void initialize(){ swerveSubsystem.resetPose(); - initial_heading = swerveSubsystem.getHeading(); isTurnFinished = false; } From 86d99f89498ba9fcff25e600b862e6400adb915d Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Thu, 4 Apr 2024 10:23:20 -0400 Subject: [PATCH 07/22] Actually shoot --- src/main/java/frc/robot/commands/ShootCommand.java | 11 +++++++---- .../robot/commands/auto/ShootAndDriveAutoCommand.java | 5 +++-- .../frc/robot/commands/auto/ShootAutoCommand.java | 5 +++-- .../robot/commands/auto/SlideAndShootAutoCommand.java | 5 +++-- .../robot/commands/auto/TurnAndShootAutoCommand.java | 5 +++-- .../frc/robot/commands/auto/TwoRingAutoCommand.java | 4 ++-- 6 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index d66c2da..350b883 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -1,14 +1,17 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class ShootCommand extends SequentialCommandGroup { - public ShootCommand ( - ShooterSubsystem shooterSubsystem - ) { + public ShootCommand (ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { addCommands( new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75), - new ShooterCommand(shooterSubsystem, "all") + new ParallelCommandGroup( + new ShooterCommand(shooterSubsystem, "both"), + new FeedShooterCommand(intakeSubsystem) + ) ); } } diff --git a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java index eb60d24..2fdf7a1 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java @@ -13,14 +13,15 @@ import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.TurnThetaCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class ShootAndDriveAutoCommand extends SequentialCommandGroup { - public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) { + public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { super( new WaitCommand(delay), - new ShootCommand(shoot).withTimeout(4), + new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(3), diff --git a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java index aa58917..7b6e429 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java @@ -9,14 +9,15 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class ShootAutoCommand extends SequentialCommandGroup { - public ShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) { + public ShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { super( new WaitCommand(delay), - new ShootCommand(shoot).withTimeout(4), + new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0) ); } diff --git a/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java index e985134..047502d 100644 --- a/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/SlideAndShootAutoCommand.java @@ -12,17 +12,18 @@ import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class SlideAndShootAutoCommand extends SequentialCommandGroup { - public SlideAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, int direction) { + public SlideAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, int direction) { super( new WaitCommand(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(direction == 0 ? -.3 : .3), supplyDouble(0), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(0)) .withTimeout(1), - new ShootCommand(shoot), + new ShootCommand(shoot, intake), new SwerveDriveCommand(swerve, supplyDouble(direction == 0 ? .3 : -.3), supplyDouble(0), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(0)) .withTimeout(1), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(0)) diff --git a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java index d6b67df..91faeb8 100644 --- a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java @@ -13,18 +13,19 @@ import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.TurnThetaCommand; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; public class TurnAndShootAutoCommand extends SequentialCommandGroup { - public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double turnDegrees, double delay) { + public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double turnDegrees, double delay) { super( new WaitCommand(delay), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(1.9), new TurnThetaCommand(swerve, turnDegrees).withTimeout(1), - new ShootCommand(shoot).withTimeout(4), + new ShootCommand(shoot, intake).withTimeout(4), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(2.8) ); diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java index 1c455fd..ee16ad3 100644 --- a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -22,7 +22,7 @@ public class TwoRingAutoCommand extends SequentialCommandGroup { public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { super( new WaitCommand(delay), - new ShootCommand(shoot).withTimeout(4), + new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new ParallelCommandGroup( new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), @@ -30,7 +30,7 @@ public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Intake ).withTimeout(3), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(3.2), - new ShootCommand(shoot).withTimeout(4) + new ShootCommand(shoot, intake).withTimeout(4) ); } From 6adfb2f3002037687854a7732b9caf8c1459a673 Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Thu, 4 Apr 2024 11:25:09 -0400 Subject: [PATCH 08/22] lpolokljvntfdtrgfdhg --- src/main/java/frc/robot/RobotContainer.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2358e65..073f1e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,19 +90,19 @@ public RobotContainer() { m_delay = m_tab.add("Delay", 0).getEntry(); m_chosenAuto.setDefaultOption("Shoot and drive from middle", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0))); m_chosenAuto.addOption("Shoot and drive from left", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0))); m_chosenAuto.addOption("Shoot and drive from right", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0))); m_chosenAuto.addOption("Drive, turn, and shoot from left", - new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), 55, m_delay.getDouble(0))); + new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), 55, m_delay.getDouble(0))); m_chosenAuto.addOption("Drive, turn, and shoot from right", - new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), -55, m_delay.getDouble(0))); + new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), -55, m_delay.getDouble(0))); m_chosenAuto.addOption("Drive back only", new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); m_chosenAuto.addOption("Shoot only", - new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(), m_delay.getDouble(0)).withTimeout(4)); + new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0)).withTimeout(4)); m_chosenAuto.addOption("2 rings", new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0))); @@ -145,7 +145,7 @@ private void configureButtonBindings() { // Auxillary stuff new JoystickButton(m_auxController, XboxController.Button.kRightBumper.value) // Shoot - .whileTrue(new ShootCommand(m_shooterSubsystem)); + .whileTrue(new ShootCommand(m_shooterSubsystem, m_intakeSubsystem)); new JoystickButton(m_auxController, XboxController.Button.kLeftBumper.value) // Intake .whileTrue(new IntakeCommand(m_shooterSubsystem)) .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); From fe1a6354a9ff672855975e3e6ebd0d38fb3c19c0 Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Thu, 4 Apr 2024 11:32:42 -0400 Subject: [PATCH 09/22] intake right reversed direction --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e79e046..d0d1b10 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -147,7 +147,7 @@ public static final class ShooterConstants { public static final double kFrontShootPower = 1.0; public static final double kBackShootPower = -1.0; public static final double kFrontIntakePower = -0.35; - public static final double kBackIntakePower = -0.30; + public static final double kBackIntakePower = 0.35; public static final double kIndexIntakePower = 0.1; public static final double kIndexPower = -1.0; public static final double kWinchUpPower = 0.5; From d8471a6de3d3a10222e3f572c3a0a3deb866036e Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Thu, 4 Apr 2024 17:43:34 -0400 Subject: [PATCH 10/22] Commit --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/GroundIntakeCommand.java | 6 +++++- .../java/frc/robot/commands/auto/TwoRingAutoCommand.java | 6 +++++- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 5 +++++ src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 +- 6 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d0d1b10..9d713be 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -110,7 +110,7 @@ public static final class DriveConstants { public static final double kTeleMaxMetersPerSec = 0.3; public static final double kFastTeleMaxMetersPerSec = 1.0; public static final double kFasterTeleMaxMetersPerSec = 1.8; - public static final double kNudgeSpeed = 0.5; + public static final double kNudgeSpeed = 0.8; public static final Button kTestMotorButton = Button.kLeftBumper; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 073f1e4..f9689e5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,9 +129,9 @@ public void robotInit() { private void configureButtonBindings() { // Driver stuff - new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) // Reset gyro + new JoystickButton(m_driverController, XboxController.Button.kStart.value) // Reset gyro .whileTrue(new ZeroHeadingCommand(m_robotDrive)); - new JoystickButton(m_driverController, XboxController.Button.kA.value) + new JoystickButton(m_driverController, XboxController.Button.kRightBumper.value) .whileTrue(new GroundIntakeCommand(m_intakeSubsystem)); new JoystickButton(m_driverController, XboxController.Button.kB.value) .whileTrue(new ExpelRingCommand(m_intakeSubsystem)); diff --git a/src/main/java/frc/robot/commands/GroundIntakeCommand.java b/src/main/java/frc/robot/commands/GroundIntakeCommand.java index 6b21c59..55171a0 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeCommand.java @@ -4,6 +4,9 @@ package frc.robot.commands; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; @@ -30,6 +33,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_subsystem.m_intakeLimitSwitch.isPressed(); + // return false; } } diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java index ee16ad3..1df4305 100644 --- a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.GroundIntakeCommand; +import frc.robot.commands.RetractIntakeCommand; import frc.robot.commands.SetPoseCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.SwerveDriveCommand; @@ -28,7 +29,10 @@ public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Intake new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), new GroundIntakeCommand(intake) ).withTimeout(3), - new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) + new ParallelCommandGroup( + new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)), + new RetractIntakeCommand(intake) + ) .withTimeout(3.2), new ShootCommand(shoot, intake).withTimeout(4) ); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1ad78ad..b3993e4 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -5,6 +5,7 @@ import com.revrobotics.SparkRelativeEncoder.Type; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkLimitSwitch; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -18,6 +19,8 @@ public class IntakeSubsystem extends SubsystemBase { public final CANSparkMax m_intakeMotor; public final RelativeEncoder m_flippyEncoder; + public final SparkLimitSwitch m_intakeLimitSwitch; + public final ShuffleboardTab m_tab; public IntakeSubsystem() { @@ -30,6 +33,8 @@ public IntakeSubsystem() { m_flippyMotor.setIdleMode(IdleMode.kCoast); m_intakeMotor.setIdleMode(IdleMode.kBrake); + m_intakeLimitSwitch = m_intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + m_tab = Shuffleboard.getTab("Intake"); m_tab.addNumber("Intake Current", this::getIntakeCurrent); m_tab.addNumber("Flipper Position", this::getFlipperEncoder); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a01c58b..faa1370 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -45,7 +45,7 @@ public ShooterSubsystem() { m_frontMotor.setIdleMode(IdleMode.kCoast); m_backMotor.setIdleMode(IdleMode.kCoast); // m_indexMotor.setIdleMode(IdleMode.kCoast); - m_winchMotor.setIdleMode(IdleMode.kCoast); + m_winchMotor.setIdleMode(IdleMode.kBrake); m_frontMotor.setInverted(false); m_backMotor.setInverted(false); // m_indexMotor.setInverted(true); From f07248dfc85bd3c5b92a8505748cb158d1c63be2 Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Thu, 4 Apr 2024 19:12:37 -0400 Subject: [PATCH 11/22] Miami Regional Day 1 1.Added limit switch functionality 2.Added automatic retraction once limit switch is pressed 3.Winchy is now squinchy 4.Lowered Winchy height 5.Two Ring auto worky --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9d713be..a1d76f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,7 +155,7 @@ public static final class ShooterConstants { public static final double kWinchDeadBand = 0.05; public static final double kWinchUpPreset = 0; - public static final double kWinchDownPreset = 1.5; + public static final double kWinchDownPreset = 1.75;//was 1.5 } public static final class ClimberConstants { From f968518c5f098523a6988335a8a9ffc79677a029 Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Thu, 4 Apr 2024 20:27:16 -0400 Subject: [PATCH 12/22] squishy intake --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/commands/ShootCommand.java | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9689e5..4770460 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,8 @@ public class RobotContainer { // The driver's controller XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); - XboxController m_auxController = new XboxController(OIConstants.kSecondControllerPort); + XboxController + m_auxController = new XboxController(OIConstants.kSecondControllerPort); // Subsystems private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index 350b883..49e13dd 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -7,8 +7,9 @@ public class ShootCommand extends SequentialCommandGroup { public ShootCommand (ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { addCommands( - new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75), - new ParallelCommandGroup( + new AirIntakeCommand(intakeSubsystem).withTimeout(0.2), + new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75), + new ParallelCommandGroup( new ShooterCommand(shooterSubsystem, "both"), new FeedShooterCommand(intakeSubsystem) ) From 1652f5c3483dd07f4cba854fcccb6349014b0cc9 Mon Sep 17 00:00:00 2001 From: Fyrefyte Date: Sat, 6 Apr 2024 06:48:05 -0400 Subject: [PATCH 13/22] Added velocity and neo free speed --- src/main/java/frc/robot/Constants.java | 10 +++--- .../robot/subsystems/ShooterSubsystem.java | 36 +++++++++---------- 2 files changed, 21 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a1d76f3..45b4dde 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -144,15 +144,15 @@ public static final class ShooterConstants { public static final int kIndexMotorCanId = 53; public static final int kWinchMotorCanId = 54; + public static final double kVortexFreeSpeed = 6000; public static final double kFrontShootPower = 1.0; - public static final double kBackShootPower = -1.0; + public static final double kBackShootPower = 1.0; public static final double kFrontIntakePower = -0.35; - public static final double kBackIntakePower = 0.35; - public static final double kIndexIntakePower = 0.1; - public static final double kIndexPower = -1.0; + public static final double kBackIntakePower = -0.35; + public static final double kWinchUpPower = 0.5; public static final double kWinchDownPower = -1.0; - + public static final double kWinchDeadBand = 0.05; public static final double kWinchUpPreset = 0; public static final double kWinchDownPreset = 1.75;//was 1.5 diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index faa1370..abcd648 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -2,9 +2,11 @@ // NEGATIVE CLOCKWISE package frc.robot.subsystems; +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.SparkRelativeEncoder.Type; @@ -21,18 +23,10 @@ public class ShooterSubsystem extends SubsystemBase { private final ShuffleboardTab m_shuffleboardtab = Shuffleboard.getTab("Shooter"); - private final GenericEntry m_frontMotorPower; - private final GenericEntry m_backMotorPower; - private final GenericEntry m_frontIntakePower; - private final GenericEntry m_backIntakePower; - // private final GenericEntry m_indexPower; - // private final GenericEntry m_indexIntakePower; - private final GenericEntry m_winchUpPower; - private final GenericEntry m_winchDownPower; + private final GenericEntry m_frontMotorPower, m_backMotorPower, m_frontIntakePower, m_backIntakePower, m_winchUpPower, m_winchDownPower; - public final CANSparkFlex m_frontMotor; - public final CANSparkFlex m_backMotor; - // public final CANSparkMax m_indexMotor; + public final CANSparkFlex m_frontMotor, m_backMotor; + public final SparkPIDController m_frontPID, m_backPID; public final CANSparkMax m_winchMotor; public final RelativeEncoder m_winchEncoder; @@ -40,6 +34,12 @@ public ShooterSubsystem() { m_frontMotor = new CANSparkFlex(ShooterConstants.kFrontMotorCanId, MotorType.kBrushless); m_backMotor = new CANSparkFlex(ShooterConstants.kBackMotorCanId, MotorType.kBrushless); + m_frontMotor.setInverted(false); + m_backMotor.setInverted(true); + m_frontPID = m_frontMotor.getPIDController(); + m_backPID = m_backMotor.getPIDController(); + m_frontPID.setReference(0, CANSparkBase.ControlType.kVelocity); + m_backPID.setReference(0, CANSparkBase.ControlType.kVelocity); // m_indexMotor = new CANSparkMax(ShooterConstants.kIndexMotorCanId, MotorType.kBrushless); m_winchMotor = new CANSparkMax(ShooterConstants.kWinchMotorCanId, MotorType.kBrushed); m_frontMotor.setIdleMode(IdleMode.kCoast); @@ -64,10 +64,6 @@ public ShooterSubsystem() { .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_backIntakePower = m_shuffleboardtab.add("Back Intake Power", ShooterConstants.kBackIntakePower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - // m_indexPower = m_shuffleboardtab.add("Index Power", ShooterConstants.kIndexPower) - // .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - // m_indexIntakePower = m_shuffleboardtab.add("Index Intake Power", ShooterConstants.kIndexIntakePower) - // .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_winchUpPower = m_shuffleboardtab.add("Winch Up Power", ShooterConstants.kWinchUpPower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", ShooterConstants.kWinchDownPower) @@ -80,14 +76,14 @@ public ShooterSubsystem() { // Shooter stuff public double[] getShooterPowers() { // Function to get the motor powers from shuffleboard and clamp them to a value between -1 and 1 return new double[] { - clampPower(m_frontMotorPower.getDouble(ShooterConstants.kFrontShootPower)), - clampPower(m_backMotorPower.getDouble(ShooterConstants.kBackShootPower)) + clampPower(m_frontMotorPower.getDouble(ShooterConstants.kFrontShootPower)*ShooterConstants.kVortexFreeSpeed), + clampPower(m_backMotorPower.getDouble(ShooterConstants.kBackShootPower)*ShooterConstants.kVortexFreeSpeed) }; } public double[] getIntakePowers() { return new double[] { - clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)), - clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)), + clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)*ShooterConstants.kVortexFreeSpeed), + clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)*ShooterConstants.kVortexFreeSpeed) // clampPower(m_indexIntakePower.getDouble(ShooterConstants.kIndexIntakePower)) }; } @@ -97,7 +93,7 @@ public void runShooter() { } public void runShooter(double rearPower) { m_frontMotor.set(getShooterPowers()[0]); - m_backMotor.set(rearPower); + m_backMotor.set(rearPower*ShooterConstants.kVortexFreeSpeed); } public void runIntake() { m_frontMotor.set(getIntakePowers()[0]); From 2639172cd5ca9137b95ff0b1456c9eaea1927693 Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Sat, 6 Apr 2024 07:20:44 -0400 Subject: [PATCH 14/22] Rebound aux controller and made TurnByTheta --- src/main/java/frc/robot/Constants.java | 4 + src/main/java/frc/robot/RobotContainer.java | 13 +- .../robot/commands/TurnByThetaCommand.java | 142 ++++++++++++++++++ 3 files changed, 155 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TurnByThetaCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a1d76f3..5843e5e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,6 +27,10 @@ public static final class DriveConstants { // TODO turn field oriented on or off public static final boolean kFieldOriented = true; + + //Turn theta + public static final double kTurnThetaMaxSpeed = 0.9; + public static final double kTurnThetaShutoffSensitivity = 0.005; // Distance between centers of right and left wheels on robot public static final double kTrackWidth = Units.inchesToMeters(24.75); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4770460..644c764 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.commands.ResetWinchCommand; import frc.robot.commands.RetractIntakeCommand; import frc.robot.commands.ShootCommand; +import frc.robot.commands.ShooterCommand; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -150,16 +151,20 @@ private void configureButtonBindings() { new JoystickButton(m_auxController, XboxController.Button.kLeftBumper.value) // Intake .whileTrue(new IntakeCommand(m_shooterSubsystem)) .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); - new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up - .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); - new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down - .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); + // new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up + // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); + // new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down + // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); new JoystickButton(m_auxController, XboxController.Button.kY.value) // Winch up preset .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchUpPreset)); new JoystickButton(m_auxController, XboxController.Button.kX.value) // Winch down preset .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchDownPreset)); new JoystickButton(m_auxController, XboxController.Button.kBack.value) // Reset winch encoder .whileTrue(new ResetWinchCommand(m_shooterSubsystem)); + new JoystickButton(m_auxController, XboxController.Button.kA.value) // Manually spin up shooter + .whileTrue(new ShooterCommand(m_shooterSubsystem, "both")); + new JoystickButton(m_auxController, XboxController.Button.kB.value) // Manually Feed Ring to shooter + .whileTrue(new ExpelRingCommand(m_intakeSubsystem)); // POV = Winch // Joysticks = Manual climbers } diff --git a/src/main/java/frc/robot/commands/TurnByThetaCommand.java b/src/main/java/frc/robot/commands/TurnByThetaCommand.java new file mode 100644 index 0000000..ec7c8f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurnByThetaCommand.java @@ -0,0 +1,142 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.OIConstants; +import frc.robot.subsystems.SwerveSubsystem; +import java.util.function.Supplier; + +public class TurnByThetaCommand extends Command { + + public final SwerveSubsystem swerveSubsystem; + public final Supplier xSpdFunction, ySpdFunction, turningSpdFunction; + public final Supplier fieldOrientedFunction, fastModeFunction; + public final SlewRateLimiter xLimiter, yLimiter, turningLimiter; + public final Supplier fieldOriented = ()-> false; + public final Supplier fastMode = ()-> false; + public final Supplier xSupplier = ()-> 0.0; + public final Supplier ySupplier = ()-> 0.0; + public final Supplier turnSupplier = ()-> 0.0; + private boolean isTurnFinished = false; + private double targetTheta, initialHeading; + + public TurnByThetaCommand(SwerveSubsystem subsystem, double turnDegrees) { + swerveSubsystem = subsystem; + xSpdFunction = xSupplier; + ySpdFunction = ySupplier; + turningSpdFunction = turnSupplier; + fieldOrientedFunction = fieldOriented; + fastModeFunction = fastMode; + targetTheta = turnDegrees; + xLimiter = new SlewRateLimiter(DriveConstants.kXSlewRateLimit); + yLimiter = new SlewRateLimiter(DriveConstants.kYSlewRateLimit); + turningLimiter = new SlewRateLimiter(DriveConstants.kTurnSlewRateLimit); + addRequirements(swerveSubsystem); + } + + @Override + public void initialize(){ + swerveSubsystem.resetPose(); + isTurnFinished = false; + initialHeading = swerveSubsystem.getHeading(); + if(initialHeading < 0){ + initialHeading = initialHeading + 180 + 360; + } + } + + @Override + public void execute() { + //autoturny stuffs + targetTheta += initialHeading; + double currentHeading = swerveSubsystem.getHeading(); + double currentRawHeading = swerveSubsystem.getHeading(); + double theta,speed; + if(currentHeading < 0){ + currentHeading = currentHeading + 180 + 360; + } + currentHeading %= 360; + theta = targetTheta - currentHeading; + speed = theta / 45; + + if(targetTheta < 0 && initialHeading >= 0 && initialHeading < 180 + targetTheta){ + if(currentHeading < 10){ + theta -= .1; //keep robot moving over the boundery + } + if(currentHeading < 0 || currentHeading > 180 + targetTheta){ + theta = (targetTheta + 360) - currentHeading; //robot move in correct direction once over boundery + } + }else if(targetTheta > 0 && initialHeading <= 0 && initialHeading > 180 + targetTheta){ + if(currentHeading > 350){ + theta += .1; //keep robot moving over the boundery + } + if(currentHeading > 359 || currentHeading < targetTheta){ + theta = (360 - targetTheta) - currentHeading;//robot move in correct direction once over boundery + } + } + System.out.println("DeltaTheta:" + theta); + System.out.println("TargetTheta:" + targetTheta); + System.out.println("Current Heading:" + currentHeading); + + + + + // Get inputs + double xSpeed = xSpdFunction.get(); + double ySpeed = ySpdFunction.get(); + double turningSpeed = turningSpdFunction.get(); + boolean fastMode = fastModeFunction.get(); + + //Hyjack joysticks + turningSpeed = MathUtil.clamp(speed, -DriveConstants.kTurnThetaMaxSpeed, DriveConstants.kTurnThetaMaxSpeed); //You's we;come constant police + xSpeed = 0; + ySpeed = 0; + System.out.println("TurningSpeed:" + turningSpeed); + + // Death + xSpeed = Math.abs(xSpeed) > OIConstants.kDriveDeadband ? xSpeed : 0; + ySpeed = Math.abs(ySpeed) > OIConstants.kDriveDeadband ? ySpeed : 0; + turningSpeed = Math.abs(turningSpeed) > OIConstants.kDriveDeadband ? turningSpeed : 0; + + //Actual death + if(Math.abs(turningSpeed) < DriveConstants.kTurnThetaShutoffSensitivity){ //TODO may need to adjust how sensitive it is + isTurnFinished = true; + } + + // Slew soup + double maxDriveSpeed = fastMode ? DriveConstants.kFastTeleMaxMetersPerSec : DriveConstants.kTeleMaxMetersPerSec; + double maxTurnSpeed = fastMode ? DriveConstants.kFastTeleMaxRadiansPerSec : DriveConstants.kTeleMaxRadiansPerSec; + xSpeed = xLimiter.calculate(xSpeed) * maxDriveSpeed; + ySpeed = yLimiter.calculate(ySpeed) * maxDriveSpeed; + turningSpeed = turningLimiter.calculate(turningSpeed) * maxTurnSpeed; + + // I am speed + ChassisSpeeds chassisSpeeds; + if (fieldOrientedFunction.get()) { + // Field oriented + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d()); + } + else { + chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed); + } + + // Divide and conker + SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + + // Actually do the thing + swerveSubsystem.setModuleStates(moduleStates); + } + + @Override + public void end(boolean interrupted) { + swerveSubsystem.stopModules(); + } + + @Override + public boolean isFinished() { + return isTurnFinished; + } +} From a981b604c5ae26dd6d2384a37d4d77b8174c078a Mon Sep 17 00:00:00 2001 From: Bad Robots Driver Station Date: Sat, 6 Apr 2024 08:25:12 -0400 Subject: [PATCH 15/22] Rebind and proper shoot command --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++------ .../java/frc/robot/commands/ShootCommand.java | 6 ++++-- .../robot/commands/auto/DriveAutoCommand.java | 2 +- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4770460..854504b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.commands.ResetWinchCommand; import frc.robot.commands.RetractIntakeCommand; import frc.robot.commands.ShootCommand; +import frc.robot.commands.ShooterCommand; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -136,8 +137,7 @@ private void configureButtonBindings() { .whileTrue(new GroundIntakeCommand(m_intakeSubsystem)); new JoystickButton(m_driverController, XboxController.Button.kB.value) .whileTrue(new ExpelRingCommand(m_intakeSubsystem)); - new JoystickButton(m_driverController, XboxController.Button.kX.value) - .whileTrue(new FeedShooterCommand(m_intakeSubsystem)); + // Left bumper = Toggle fastmode // Left trigger = Toggle fastermode // POV = Nudge @@ -150,16 +150,21 @@ private void configureButtonBindings() { new JoystickButton(m_auxController, XboxController.Button.kLeftBumper.value) // Intake .whileTrue(new IntakeCommand(m_shooterSubsystem)) .whileTrue(new AirIntakeCommand(m_intakeSubsystem)); - new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up - .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); - new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down - .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); + // new JoystickButton(m_auxController, XboxController.Button.kB.value) // Climber up + // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberUpPower)); + // new JoystickButton(m_auxController, XboxController.Button.kA.value) // Climber down + // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); new JoystickButton(m_auxController, XboxController.Button.kY.value) // Winch up preset .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchUpPreset)); new JoystickButton(m_auxController, XboxController.Button.kX.value) // Winch down preset .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchDownPreset)); new JoystickButton(m_auxController, XboxController.Button.kBack.value) // Reset winch encoder .whileTrue(new ResetWinchCommand(m_shooterSubsystem)); + + new JoystickButton(m_auxController, XboxController.Button.kA.value) + .whileTrue(new FeedShooterCommand(m_intakeSubsystem)); + new JoystickButton(m_auxController, XboxController.Button.kB.value) + .whileTrue(new ShooterCommand(m_shooterSubsystem)); // POV = Winch // Joysticks = Manual climbers } diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index 49e13dd..09c62e8 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -7,9 +7,11 @@ public class ShootCommand extends SequentialCommandGroup { public ShootCommand (ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { addCommands( + new ParallelCommandGroup( new AirIntakeCommand(intakeSubsystem).withTimeout(0.2), - new ShooterCommand(shooterSubsystem, "both").withTimeout(1.75), - new ParallelCommandGroup( + new ShooterCommand(shooterSubsystem, "both") + ).withTimeout(1.75), + new ParallelCommandGroup( new ShooterCommand(shooterSubsystem, "both"), new FeedShooterCommand(intakeSubsystem) ) diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java index cc0144d..d8f6642 100644 --- a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java @@ -21,7 +21,7 @@ public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d s new WaitCommand(delay), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) - .withTimeout(3), + .withTimeout(7), new TurnThetaCommand(swerve, 0) ); } From 6f22332c34007442d0939a5712df1fac2a3e4631 Mon Sep 17 00:00:00 2001 From: Bad Robots Driver Station Date: Sat, 6 Apr 2024 09:11:28 -0400 Subject: [PATCH 16/22] Wok --- src/main/java/frc/robot/Constants.java | 4 ++-- .../frc/robot/subsystems/ShooterSubsystem.java | 15 ++++++++++++++- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a2f0138..469d147 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -150,9 +150,9 @@ public static final class ShooterConstants { public static final double kVortexFreeSpeed = 6000; public static final double kFrontShootPower = 1.0; - public static final double kBackShootPower = 1.0; + public static final double kBackShootPower = -1.0; public static final double kFrontIntakePower = -0.35; - public static final double kBackIntakePower = -0.35; + public static final double kBackIntakePower = 0.35; public static final double kWinchUpPower = 0.5; public static final double kWinchDownPower = -1.0; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index abcd648..16a9423 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -35,7 +35,7 @@ public ShooterSubsystem() { m_frontMotor = new CANSparkFlex(ShooterConstants.kFrontMotorCanId, MotorType.kBrushless); m_backMotor = new CANSparkFlex(ShooterConstants.kBackMotorCanId, MotorType.kBrushless); m_frontMotor.setInverted(false); - m_backMotor.setInverted(true); + m_backMotor.setInverted(false); m_frontPID = m_frontMotor.getPIDController(); m_backPID = m_backMotor.getPIDController(); m_frontPID.setReference(0, CANSparkBase.ControlType.kVelocity); @@ -69,6 +69,10 @@ public ShooterSubsystem() { m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", ShooterConstants.kWinchDownPower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + m_shuffleboardtab.addBoolean("Shooter spun up", this::getSpunUp); + m_shuffleboardtab.addNumber("Front motor RPM", this::getFrontRPM); + m_shuffleboardtab.addNumber("Back motor RPM", this::getBackRPM); + // Winch encoder value m_shuffleboardtab.addNumber("Winch Encoder", this::getWinchEncoder); } @@ -112,6 +116,15 @@ public boolean isShooterRunning() { return true; //Otherwise, the shooter is running } } + public double getFrontRPM() { + return m_frontMotor.getEncoder().getVelocity(); + } + public double getBackRPM() { + return m_backMotor.getEncoder().getVelocity(); + } + public boolean getSpunUp() { + return m_frontMotor.getEncoder().getVelocity() >= ShooterConstants.kVortexFreeSpeed && m_backMotor.getEncoder().getVelocity() >= ShooterConstants.kVortexFreeSpeed; + } // Indexer stuff // public double getIndexPower() {return clampPower(m_indexPower.getDouble(ShooterConstants.kIndexPower));} From abcae119a4066637be4ac806e85e2b11af946546 Mon Sep 17 00:00:00 2001 From: Bad Robots Driver Station Date: Sat, 6 Apr 2024 09:18:24 -0400 Subject: [PATCH 17/22] "Shooter spun up" fix --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 16a9423..4cafbea 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -123,7 +123,8 @@ public double getBackRPM() { return m_backMotor.getEncoder().getVelocity(); } public boolean getSpunUp() { - return m_frontMotor.getEncoder().getVelocity() >= ShooterConstants.kVortexFreeSpeed && m_backMotor.getEncoder().getVelocity() >= ShooterConstants.kVortexFreeSpeed; + return Math.abs(m_frontMotor.getEncoder().getVelocity()) >= ShooterConstants.kVortexFreeSpeed + && Math.abs(m_backMotor.getEncoder().getVelocity()) >= ShooterConstants.kVortexFreeSpeed; } // Indexer stuff From 91c8c1a32db231e3bec17e52f7cc2144ceeed7de Mon Sep 17 00:00:00 2001 From: Bad Robots Driver Station Date: Sat, 6 Apr 2024 09:52:09 -0400 Subject: [PATCH 18/22] Constants --- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/ShooterSubsystem.java | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 469d147..fbdc845 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -148,7 +148,7 @@ public static final class ShooterConstants { public static final int kIndexMotorCanId = 53; public static final int kWinchMotorCanId = 54; - public static final double kVortexFreeSpeed = 6000; + public static final double kShooterMaxSpeed = 1.0; public static final double kFrontShootPower = 1.0; public static final double kBackShootPower = -1.0; public static final double kFrontIntakePower = -0.35; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 4cafbea..f8eb4a4 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -80,14 +80,14 @@ public ShooterSubsystem() { // Shooter stuff public double[] getShooterPowers() { // Function to get the motor powers from shuffleboard and clamp them to a value between -1 and 1 return new double[] { - clampPower(m_frontMotorPower.getDouble(ShooterConstants.kFrontShootPower)*ShooterConstants.kVortexFreeSpeed), - clampPower(m_backMotorPower.getDouble(ShooterConstants.kBackShootPower)*ShooterConstants.kVortexFreeSpeed) + clampPower(m_frontMotorPower.getDouble(ShooterConstants.kFrontShootPower)*ShooterConstants.kShooterMaxSpeed), + clampPower(m_backMotorPower.getDouble(ShooterConstants.kBackShootPower)*ShooterConstants.kShooterMaxSpeed) }; } public double[] getIntakePowers() { return new double[] { - clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)*ShooterConstants.kVortexFreeSpeed), - clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)*ShooterConstants.kVortexFreeSpeed) + clampPower(m_frontIntakePower.getDouble(ShooterConstants.kFrontIntakePower)*ShooterConstants.kShooterMaxSpeed), + clampPower(m_backIntakePower.getDouble(ShooterConstants.kBackIntakePower)*ShooterConstants.kShooterMaxSpeed) // clampPower(m_indexIntakePower.getDouble(ShooterConstants.kIndexIntakePower)) }; } @@ -97,7 +97,7 @@ public void runShooter() { } public void runShooter(double rearPower) { m_frontMotor.set(getShooterPowers()[0]); - m_backMotor.set(rearPower*ShooterConstants.kVortexFreeSpeed); + m_backMotor.set(rearPower*ShooterConstants.kShooterMaxSpeed); } public void runIntake() { m_frontMotor.set(getIntakePowers()[0]); @@ -123,8 +123,8 @@ public double getBackRPM() { return m_backMotor.getEncoder().getVelocity(); } public boolean getSpunUp() { - return Math.abs(m_frontMotor.getEncoder().getVelocity()) >= ShooterConstants.kVortexFreeSpeed - && Math.abs(m_backMotor.getEncoder().getVelocity()) >= ShooterConstants.kVortexFreeSpeed; + return Math.abs(m_frontMotor.getEncoder().getVelocity()) >= 6000 + && Math.abs(m_backMotor.getEncoder().getVelocity()) >= 6000; } // Indexer stuff From 4170ac1b585879c0b62fe2ee816462f7084e8069 Mon Sep 17 00:00:00 2001 From: aBanjo12 Date: Sat, 6 Apr 2024 14:06:15 -0400 Subject: [PATCH 19/22] constant --- src/main/java/frc/robot/Constants.java | 5 ++ .../robot/subsystems/BlinkinSubsystem.java | 67 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/BlinkinSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a2f0138..e1c5acf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,6 +22,11 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public class BlinkinConstants { + + public static final int kBlinkinPort = 0; + + } public static final class DriveConstants { diff --git a/src/main/java/frc/robot/subsystems/BlinkinSubsystem.java b/src/main/java/frc/robot/subsystems/BlinkinSubsystem.java new file mode 100644 index 0000000..19d444c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/BlinkinSubsystem.java @@ -0,0 +1,67 @@ +// 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 frc.robot.subsystems; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.BlinkinConstants; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + + +public class BlinkinSubsystem extends SubsystemBase { + + private final ShuffleboardTab m_tab = Shuffleboard.getTab("Blinkin"); + + private final Spark blinkin; + + public BlinkinSubsystem() { + blinkin = new Spark(BlinkinConstants.kBlinkinPort); + m_tab.add(blinkin.toString(), blinkin); + } + + public BooleanSupplier supplier = () -> {return false;}; + + @Override + public void periodic() { + if (supplier.getAsBoolean()) + setGreen(); + // This method will be called once per scheduler run + } + + public void set(double pattern) { + blinkin.set(pattern); + } + + public void setOcean(){ + blinkin.set(-0.95); + } + + public void setRainbow(){ + blinkin.set(-0.99); + } + + public void setBlue(){ + blinkin.set(0.87); + } + + public void setGreen(){ + blinkin.set(0.77); + } + + public void setRed(){ + blinkin.set(0.61); + } + + public void setWhite(){ + blinkin.set(0.93); + } + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} \ No newline at end of file From 766fba41b2858df06c101e6c2694bb156dce9748 Mon Sep 17 00:00:00 2001 From: Bad Robots Driver Station Date: Sat, 6 Apr 2024 10:49:25 -0400 Subject: [PATCH 20/22] Name --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfc8e62..36b261a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -108,7 +108,7 @@ public RobotContainer() { m_chosenAuto.addOption("2 rings", new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0))); - m_tab.add(m_chosenAuto); + m_tab.add("Auto", m_chosenAuto); // Configure the button bindings configureButtonBindings(); From d23768ad9b6bb5691869685f292bd74d4fa19ee1 Mon Sep 17 00:00:00 2001 From: Bad Robots Driver Station Date: Sat, 6 Apr 2024 12:06:08 -0400 Subject: [PATCH 21/22] Supplier (doesn't work) --- src/main/java/frc/robot/RobotContainer.java | 23 +++++++++++-------- .../robot/commands/auto/DriveAutoCommand.java | 4 ++-- .../auto/ShootAndDriveAutoCommand.java | 4 ++-- .../robot/commands/auto/ShootAutoCommand.java | 6 +++-- .../auto/TurnAndShootAutoCommand.java | 4 ++-- .../commands/auto/TwoRingAutoCommand.java | 4 ++-- 6 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 36b261a..58adda0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,27 +86,27 @@ public RobotContainer() { m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed)); m_intakeSubsystem.setDefaultCommand(new RetractIntakeCommand(m_intakeSubsystem)); - // Auto chooser setup + // Auto chooser setup (sqeak!) m_tab = Shuffleboard.getTab("Auto"); m_delay = m_tab.add("Delay", 0).getEntry(); m_chosenAuto.setDefaultOption("Shoot and drive from middle", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay)); m_chosenAuto.addOption("Shoot and drive from left", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(55)), this::getDelay)); m_chosenAuto.addOption("Shoot and drive from right", - new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), m_delay.getDouble(0))); + new ShootAndDriveAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(0, 0, Rotation2d.fromDegrees(-55)), this::getDelay)); m_chosenAuto.addOption("Drive, turn, and shoot from left", - new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), 55, m_delay.getDouble(0))); + new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), 55, this::getDelay)); m_chosenAuto.addOption("Drive, turn, and shoot from right", - new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), -55, m_delay.getDouble(0))); + new TurnAndShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), -55, this::getDelay)); m_chosenAuto.addOption("Drive back only", - new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), m_delay.getDouble(0))); + new DriveAutoCommand(m_shooterSubsystem, m_robotDrive, new Pose2d(0,0,Rotation2d.fromDegrees(0)), this::getDelay)); m_chosenAuto.addOption("Shoot only", - new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0)).withTimeout(4)); + new ShootAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay).withTimeout(4)); m_chosenAuto.addOption("2 rings", - new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), m_delay.getDouble(0))); + new TwoRingAutoCommand(m_shooterSubsystem, m_robotDrive, m_intakeSubsystem, new Pose2d(), this::getDelay)); m_tab.add("Auto", m_chosenAuto); @@ -183,6 +183,11 @@ boolean getFasterMode() { else fasterMode = false; return fasterMode; } + + double getDelay() { + return m_delay.getDouble(0); + } + double getRightX() {return m_driverController.getRightX();} double getLeftX() {return -m_driverController.getLeftX();} double getLeftY() {return -m_driverController.getLeftY();} diff --git a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java index d8f6642..c81208d 100644 --- a/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/DriveAutoCommand.java @@ -16,9 +16,9 @@ import frc.robot.subsystems.SwerveSubsystem; public class DriveAutoCommand extends SequentialCommandGroup { - public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, double delay) { + public DriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(7), diff --git a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java index 2fdf7a1..01a77db 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAndDriveAutoCommand.java @@ -18,9 +18,9 @@ import frc.robot.subsystems.SwerveSubsystem; public class ShootAndDriveAutoCommand extends SequentialCommandGroup { - public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { + public ShootAndDriveAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) diff --git a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java index 7b6e429..1eddd1b 100644 --- a/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShootAutoCommand.java @@ -4,6 +4,8 @@ package frc.robot.commands.auto; +import java.util.function.Supplier; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -14,9 +16,9 @@ import frc.robot.subsystems.SwerveSubsystem; public class ShootAutoCommand extends SequentialCommandGroup { - public ShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { + public ShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0) ); diff --git a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java index 91faeb8..a29c944 100644 --- a/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TurnAndShootAutoCommand.java @@ -18,9 +18,9 @@ import frc.robot.subsystems.SwerveSubsystem; public class TurnAndShootAutoCommand extends SequentialCommandGroup { - public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double turnDegrees, double delay) { + public TurnAndShootAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double turnDegrees, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new SwerveDriveCommand(swerve, supplyDouble(0), supplyDouble(-.3), supplyDouble(0), true, supplyBoolean(true), supplyBoolean(false), supplyDouble(-1)) .withTimeout(1.9), diff --git a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java index 1df4305..8044757 100644 --- a/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/TwoRingAutoCommand.java @@ -20,9 +20,9 @@ import frc.robot.subsystems.SwerveSubsystem; public class TwoRingAutoCommand extends SequentialCommandGroup { - public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, double delay) { + public TwoRingAutoCommand(ShooterSubsystem shoot, SwerveSubsystem swerve, IntakeSubsystem intake, Pose2d startingOffset, Supplier delay) { super( - new WaitCommand(delay), + new WaitCommand(delay.get()), new ShootCommand(shoot, intake).withTimeout(4), new SetPoseCommand(swerve, startingOffset).withTimeout(0), new ParallelCommandGroup( From df7b1fa5552bf5a3b7ffbd8fdd7b57e906ede819 Mon Sep 17 00:00:00 2001 From: TheMagneticDude <110270128+TheMagneticDude@users.noreply.github.com> Date: Sun, 7 Apr 2024 15:02:14 -0400 Subject: [PATCH 22/22] Separated Winchy Squinchy Subsystem from Shooter Subsystem So now I can control the winchy without interrupting the intake/shooter --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 13 ++-- .../frc/robot/commands/ResetWinchCommand.java | 5 +- .../java/frc/robot/commands/WinchCommand.java | 5 +- .../robot/commands/WinchPresetCommand.java | 10 +-- .../robot/subsystems/ShooterSubsystem.java | 39 +---------- .../frc/robot/subsystems/WinchySubsystem.java | 67 +++++++++++++++++++ 7 files changed, 92 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/WinchySubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e9fe2b0..0c4c73e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -158,7 +158,9 @@ public static final class ShooterConstants { public static final double kBackShootPower = -1.0; public static final double kFrontIntakePower = -0.35; public static final double kBackIntakePower = 0.35; + } + public static final class WinchySquinchyConstants{ public static final double kWinchUpPower = 0.5; public static final double kWinchDownPower = -1.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 58adda0..4e05cab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.WinchySquinchyConstants; import frc.robot.commands.SwerveDriveCommand; import frc.robot.commands.WinchCommand; import frc.robot.commands.WinchPresetCommand; @@ -30,6 +31,7 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.WinchySubsystem; import frc.robot.commands.AirIntakeCommand; import frc.robot.commands.ClimbCommand; import frc.robot.commands.ExpelRingCommand; @@ -58,6 +60,7 @@ public class RobotContainer { // Subsystems private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final WinchySubsystem m_winchySubsystem = new WinchySubsystem(); private final SwerveSubsystem m_robotDrive = new SwerveSubsystem(); private boolean fastMode = false; private boolean fasterMode = false; @@ -83,7 +86,7 @@ public RobotContainer() { this::getFasterMode, this::getPOV)); m_climberSubsystem.setDefaultCommand(new ClimbCommand(m_climberSubsystem, this::getAuxRightY, this::getAuxLeftY)); - m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_shooterSubsystem, this::POVToWinchSpeed)); + m_shooterSubsystem.setDefaultCommand(new WinchCommand(m_winchySubsystem, this::POVToWinchSpeed)); m_intakeSubsystem.setDefaultCommand(new RetractIntakeCommand(m_intakeSubsystem)); // Auto chooser setup (sqeak!) @@ -155,11 +158,11 @@ private void configureButtonBindings() { // new JoystickButton(m_auxControllser, XboxController.Button.kA.value) // Climber down // .whileTrue(new ClimbCommand(m_climberSubsystem, ClimberConstants.kClimberDownPower)); new JoystickButton(m_auxController, XboxController.Button.kY.value) // Winch up preset - .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchUpPreset)); + .whileTrue(new WinchPresetCommand(m_winchySubsystem, WinchySquinchyConstants.kWinchUpPreset)); new JoystickButton(m_auxController, XboxController.Button.kX.value) // Winch down preset - .whileTrue(new WinchPresetCommand(m_shooterSubsystem, ShooterConstants.kWinchDownPreset)); + .whileTrue(new WinchPresetCommand(m_winchySubsystem, WinchySquinchyConstants.kWinchDownPreset)); new JoystickButton(m_auxController, XboxController.Button.kBack.value) // Reset winch encoder - .whileTrue(new ResetWinchCommand(m_shooterSubsystem)); + .whileTrue(new ResetWinchCommand(m_winchySubsystem)); new JoystickButton(m_auxController, XboxController.Button.kA.value) .whileTrue(new FeedShooterCommand(m_intakeSubsystem)); @@ -196,7 +199,7 @@ boolean getFasterMode() { double getAuxLeftY() {return Math.abs(m_auxController.getLeftY()) > OIConstants.kDriveDeadband ? m_auxController.getLeftY() : 0;} double getAuxPOV() {return m_auxController.getPOV();} double POVToWinchSpeed() { - return getAuxPOV() == 0 ? ShooterConstants.kWinchUpPower : (getAuxPOV() == 180 ? ShooterConstants.kWinchDownPower : 0); + return getAuxPOV() == 0 ? WinchySquinchyConstants.kWinchUpPower : (getAuxPOV() == 180 ? WinchySquinchyConstants.kWinchDownPower : 0); } /** diff --git a/src/main/java/frc/robot/commands/ResetWinchCommand.java b/src/main/java/frc/robot/commands/ResetWinchCommand.java index 2322ee6..865842c 100644 --- a/src/main/java/frc/robot/commands/ResetWinchCommand.java +++ b/src/main/java/frc/robot/commands/ResetWinchCommand.java @@ -2,11 +2,12 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.WinchySubsystem; public class ResetWinchCommand extends Command { - private final ShooterSubsystem m_subsystem; + private final WinchySubsystem m_subsystem; - public ResetWinchCommand(ShooterSubsystem subsystem) { + public ResetWinchCommand(WinchySubsystem subsystem) { m_subsystem = subsystem; addRequirements(subsystem); } diff --git a/src/main/java/frc/robot/commands/WinchCommand.java b/src/main/java/frc/robot/commands/WinchCommand.java index 5f96b82..440f813 100644 --- a/src/main/java/frc/robot/commands/WinchCommand.java +++ b/src/main/java/frc/robot/commands/WinchCommand.java @@ -4,12 +4,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.WinchySubsystem; public class WinchCommand extends Command { - private final ShooterSubsystem m_subsystem; + private final WinchySubsystem m_subsystem; private final Supplier m_power; - public WinchCommand(ShooterSubsystem subsystem, Supplier power) { + public WinchCommand(WinchySubsystem subsystem, Supplier power) { m_subsystem = subsystem; m_power = power; addRequirements(subsystem); diff --git a/src/main/java/frc/robot/commands/WinchPresetCommand.java b/src/main/java/frc/robot/commands/WinchPresetCommand.java index a62eb7e..66cbb16 100644 --- a/src/main/java/frc/robot/commands/WinchPresetCommand.java +++ b/src/main/java/frc/robot/commands/WinchPresetCommand.java @@ -4,19 +4,21 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.WinchySquinchyConstants; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.WinchySubsystem; public class WinchPresetCommand extends Command { - private final ShooterSubsystem m_subsystem; + private final WinchySubsystem m_subsystem; private final Supplier m_goal; - public WinchPresetCommand(ShooterSubsystem subsystem, Supplier goalPosition) { + public WinchPresetCommand(WinchySubsystem subsystem, Supplier goalPosition) { m_subsystem = subsystem; m_goal = goalPosition; addRequirements(subsystem); } - public WinchPresetCommand(ShooterSubsystem subsystem, double goalPosition) { + public WinchPresetCommand(WinchySubsystem subsystem, double goalPosition) { m_subsystem = subsystem; m_goal = () -> goalPosition; addRequirements(subsystem); @@ -42,6 +44,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Math.abs(m_subsystem.getWinchEncoder() - m_goal.get()) < ShooterConstants.kWinchDeadBand; + return Math.abs(m_subsystem.getWinchEncoder() - m_goal.get()) < WinchySquinchyConstants.kWinchDeadBand; } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f8eb4a4..5dca565 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -23,12 +23,11 @@ public class ShooterSubsystem extends SubsystemBase { private final ShuffleboardTab m_shuffleboardtab = Shuffleboard.getTab("Shooter"); - private final GenericEntry m_frontMotorPower, m_backMotorPower, m_frontIntakePower, m_backIntakePower, m_winchUpPower, m_winchDownPower; + private final GenericEntry m_frontMotorPower, m_backMotorPower, m_frontIntakePower, m_backIntakePower; public final CANSparkFlex m_frontMotor, m_backMotor; public final SparkPIDController m_frontPID, m_backPID; - public final CANSparkMax m_winchMotor; - public final RelativeEncoder m_winchEncoder; + public ShooterSubsystem() { @@ -40,17 +39,11 @@ public ShooterSubsystem() { m_backPID = m_backMotor.getPIDController(); m_frontPID.setReference(0, CANSparkBase.ControlType.kVelocity); m_backPID.setReference(0, CANSparkBase.ControlType.kVelocity); - // m_indexMotor = new CANSparkMax(ShooterConstants.kIndexMotorCanId, MotorType.kBrushless); - m_winchMotor = new CANSparkMax(ShooterConstants.kWinchMotorCanId, MotorType.kBrushed); m_frontMotor.setIdleMode(IdleMode.kCoast); m_backMotor.setIdleMode(IdleMode.kCoast); - // m_indexMotor.setIdleMode(IdleMode.kCoast); - m_winchMotor.setIdleMode(IdleMode.kBrake); m_frontMotor.setInverted(false); m_backMotor.setInverted(false); - // m_indexMotor.setInverted(true); - m_winchMotor.setInverted(false); - m_winchEncoder = m_winchMotor.getEncoder(Type.kQuadrature, 8192); + // Displays whether or not the shooter is running m_shuffleboardtab.addBoolean("Shooter Running", this::isShooterRunning); @@ -64,17 +57,10 @@ public ShooterSubsystem() { .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_backIntakePower = m_shuffleboardtab.add("Back Intake Power", ShooterConstants.kBackIntakePower) .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_winchUpPower = m_shuffleboardtab.add("Winch Up Power", ShooterConstants.kWinchUpPower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); - m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", ShooterConstants.kWinchDownPower) - .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); m_shuffleboardtab.addBoolean("Shooter spun up", this::getSpunUp); m_shuffleboardtab.addNumber("Front motor RPM", this::getFrontRPM); m_shuffleboardtab.addNumber("Back motor RPM", this::getBackRPM); - - // Winch encoder value - m_shuffleboardtab.addNumber("Winch Encoder", this::getWinchEncoder); } // Shooter stuff @@ -127,25 +113,6 @@ public boolean getSpunUp() { && Math.abs(m_backMotor.getEncoder().getVelocity()) >= 6000; } - // Indexer stuff - // public double getIndexPower() {return clampPower(m_indexPower.getDouble(ShooterConstants.kIndexPower));} - // public void runIndex() {m_indexMotor.set(getIndexPower());} - // public void stopIndex() {m_indexMotor.stopMotor();} - - // Winch stuff - public double[] getWinchPowers() { - return new double[] { - clampPower(m_winchUpPower.getDouble(ShooterConstants.kWinchUpPower)), - clampPower(m_winchDownPower.getDouble(ShooterConstants.kWinchDownPower)) - }; - } - public void winchUp() {m_winchMotor.set(getWinchPowers()[0]);} - public void winchDown() {m_winchMotor.set(getWinchPowers()[1]);} - public void stopWinch() {m_winchMotor.stopMotor();} - public void manualWinch(double power) {m_winchMotor.set(power);} - public double getWinchEncoder() {return m_winchEncoder.getPosition();} - public void resetWinchEncoder() {m_winchEncoder.setPosition(0);} - // Function to clamp the power to a value between -1 and 1 public static double clampPower(double power) { return MathUtil.clamp(power, -1, 1); diff --git a/src/main/java/frc/robot/subsystems/WinchySubsystem.java b/src/main/java/frc/robot/subsystems/WinchySubsystem.java new file mode 100644 index 0000000..24ac4a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/WinchySubsystem.java @@ -0,0 +1,67 @@ +// POSITIVE COUNTERCLOCKWISE +// NEGATIVE CLOCKWISE +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkRelativeEncoder.Type; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.WinchySquinchyConstants; + +import java.util.Map; + +public class WinchySubsystem extends SubsystemBase { + + private final ShuffleboardTab m_shuffleboardtab = Shuffleboard.getTab("Winchy"); + + private final GenericEntry m_winchUpPower, m_winchDownPower; + + public final CANSparkMax m_winchMotor; + public final RelativeEncoder m_winchEncoder; + + public WinchySubsystem() { + m_winchMotor = new CANSparkMax(ShooterConstants.kWinchMotorCanId, MotorType.kBrushed); + m_winchMotor.setIdleMode(IdleMode.kBrake); + m_winchMotor.setInverted(false); + m_winchEncoder = m_winchMotor.getEncoder(Type.kQuadrature, 8192); + + // Shuffleboard numbers for the winchy squinchy + m_winchUpPower = m_shuffleboardtab.add("Winch Up Power", WinchySquinchyConstants.kWinchUpPower) + .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + m_winchDownPower = m_shuffleboardtab.add("Winch Down Power", WinchySquinchyConstants.kWinchDownPower) + .withProperties(Map.of("min", -1.0, "max", 1.0)).getEntry(); + + // Winch encoder value + m_shuffleboardtab.addNumber("Winch Encoder", this::getWinchEncoder); + } + + // Winch stuff + public double[] getWinchPowers() { + return new double[] { + clampPower(m_winchUpPower.getDouble(WinchySquinchyConstants.kWinchUpPower)), + clampPower(m_winchDownPower.getDouble(WinchySquinchyConstants.kWinchDownPower)) + }; + } + public void winchUp() {m_winchMotor.set(getWinchPowers()[0]);} + public void winchDown() {m_winchMotor.set(getWinchPowers()[1]);} + public void stopWinch() {m_winchMotor.stopMotor();} + public void manualWinch(double power) {m_winchMotor.set(power);} + public double getWinchEncoder() {return m_winchEncoder.getPosition();} + public void resetWinchEncoder() {m_winchEncoder.setPosition(0);} + + // Function to clamp the power to a value between -1 and 1 + public static double clampPower(double power) { + return MathUtil.clamp(power, -1, 1); + } +}