From 0e12f00f6d71a50c4f4205105cb090abd3b4792f Mon Sep 17 00:00:00 2001 From: var Cepheid Date: Thu, 17 Mar 2022 21:03:29 -0700 Subject: [PATCH 1/7] moved constants to Constants and other code cleanup --- .../java/com/pigmice/frc/robot/Constants.java | 9 ++++- .../commands/shooter/ShootBallCommand.java | 18 +++------- .../pigmice/frc/robot/subsystems/Shooter.java | 34 +++++++++---------- 3 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/pigmice/frc/robot/Constants.java b/src/main/java/com/pigmice/frc/robot/Constants.java index d356288..ce992f1 100644 --- a/src/main/java/com/pigmice/frc/robot/Constants.java +++ b/src/main/java/com/pigmice/frc/robot/Constants.java @@ -24,6 +24,12 @@ public static class ShooterConfig { public static final double topMotorSpeed = 0.62; public static final double bottomMotorSpeed = 0.62; + + public static final double shooterP = .02D; + public static final double shooterS = 0; + public static final double shooterV = .5; + + public static final double velocityThreshhold = 100; // tune this } public static class ClimberConfig { @@ -151,7 +157,8 @@ public static class IndexerConfig { public static class VisionConfig { public static final double cameraHeightMeters = 0.8; - public static final double targetHeightMeters = 2.38;// Units.inchesToMeters(104); // 8'8" + public static final double targetHeightMeters = 2.38; + // Units.inchesToMeters(104); // 8'8" public static final double cameraPitchRadians = 0; public static final double goalRangeMeters = 0.0; diff --git a/src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java b/src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java index 3b014a3..c96132d 100644 --- a/src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java +++ b/src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java @@ -8,26 +8,18 @@ import com.pigmice.frc.robot.subsystems.Indexer; import com.pigmice.frc.robot.subsystems.Shooter; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.*; public class ShootBallCommand extends SequentialCommandGroup { - private final Shooter shooter; - public ShootBallCommand(Shooter shooter, Indexer indexer) { - super( + super( new InstantCommand(() -> shooter.enable()), new WaitUntilCommand(shooter::isAtTargetVelocity), new InstantCommand(() -> indexer.resetEncoder()), new InstantCommand(() -> indexer.enable()), - new SpinIndexerToAngle(indexer, 200, false) - ); - - this.shooter = shooter; + new SpinIndexerToAngle(indexer, 200, false)); - addRequirements(shooter); - addRequirements(indexer); + addRequirements(shooter); + addRequirements(indexer); } } \ No newline at end of file diff --git a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java index c34fb61..0f4dc07 100644 --- a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java +++ b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java @@ -19,13 +19,10 @@ public class Shooter extends SubsystemBase { private TalonSRX topMotor; private TalonSRX botMotor; - private final double SHOOTER_KP = .02D; + private double shooterP, shooterS, shooterV, vThresh; - private final double SHOOTER_KS = 0; - private final double SHOOTER_KV = .5; - - private final RPMPController topController = new RPMPController(SHOOTER_KP, 0.25); - private final RPMPController botController = new RPMPController(SHOOTER_KP, 0.25); + private final RPMPController topController = new RPMPController(shooterP, 0.25); + private final RPMPController botController = new RPMPController(shooterP, 0.25); private final ShuffleboardTab shooterTab; @@ -50,9 +47,6 @@ public class Shooter extends SubsystemBase { // private static final double MAX_RPM_775 = 18700; // private static final double MAX_RPS_775 = MAX_RPM_775 / 60; - // tune this - private static final double VELOCITY_THRESHOLD = 100; - private boolean atTarget = false; // Create a new Shooter @@ -71,6 +65,11 @@ public Shooter() { // topMotor.setNeutralMode(NeutralMode.Coast); // botMotor.setNeutralMode(NeutralMode.Coast); + this.shooterP = ShooterConfig.shooterP; + this.shooterS = ShooterConfig.shooterS; + this.shooterV = ShooterConfig.shooterV; + this.vThresh = ShooterConfig.velocityThreshhold; + this.shooterTab = Shuffleboard.getTab("Shooter"); this.topRPMEntry = shooterTab.add("Top RPM", 1).getEntry(); @@ -103,12 +102,13 @@ public void setEnabled(boolean enabled) { @Override public void periodic() { - if (!enabled) + if (!enabled) { this.setTargetSpeeds(0, 0); - else - this.setTargetSpeeds(2400, 2000); + } else { + this.setTargetSpeeds(2400, 2000); + } - //this.setTargetSpeeds(2600, 2300); + // this.setTargetSpeeds(2600, 2300); // this.setTargetSpeeds(2000, 1750); double topRPM = this.topTargetRPM == RPM_NOT_SET ? this.topRPMEntry.getDouble(ShooterConfig.topMotorSpeed) @@ -137,8 +137,8 @@ public void periodic() { topCalculated.setDouble(topTarget); botCalculated.setDouble(botTarget); - this.atTarget = Math.abs(topRPM - topActualRPM) <= VELOCITY_THRESHOLD - && Math.abs(botRPM - botActualRPM) <= VELOCITY_THRESHOLD; + this.atTarget = Math.abs(topRPM - topActualRPM) <= vThresh + && Math.abs(botRPM - botActualRPM) <= vThresh; this.atTargetEntry.setBoolean(this.atTarget); @@ -175,11 +175,11 @@ public void stopMotors() { } public boolean isAtTargetVelocity() { - return topTargetRPM != 0 && topTargetRPM != 0 && this.atTarget; + return topTargetRPM != 0 && botTargetRPM != 0 && this.atTarget; } private double calculate(double velocity) { - return SHOOTER_KS * Math.signum(velocity) + SHOOTER_KV * velocity; + return shooterS * Math.signum(velocity) + shooterV * velocity; } @Override From 9240ead591f45ea86f88de2ad9cf76ce280c45ab Mon Sep 17 00:00:00 2001 From: ev118 Date: Fri, 18 Mar 2022 16:58:53 -0700 Subject: [PATCH 2/7] save --- .../pigmice/frc/robot/subsystems/Shooter.java | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java index 0f4dc07..c02d914 100644 --- a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java +++ b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java @@ -84,21 +84,10 @@ public Shooter() { this.atTargetEntry = shooterTab.add("At Target", this.atTarget).getEntry(); } - public void enable() { - setEnabled(true); - } - - public void disable() { - setEnabled(false); - } - - public void toggle() { - this.setEnabled(!this.enabled); - } - - public void setEnabled(boolean enabled) { - this.enabled = enabled; - } + public void enable() {setEnabled(true);} + public void disable() {setEnabled(false);} + public void toggle() {this.setEnabled(!this.enabled);} + public void setEnabled(boolean enabled) {this.enabled = enabled;} @Override public void periodic() { From be0844d95f686667e933993db76890920812ed7f Mon Sep 17 00:00:00 2001 From: var Cepheid Date: Fri, 18 Mar 2022 17:02:35 -0700 Subject: [PATCH 3/7] added enumerator, formatting, command file changes --- .../java/com/pigmice/frc/robot/Constants.java | 21 ++++- .../com/pigmice/frc/robot/RobotContainer.java | 78 ++++++++++--------- .../{shooter => }/ShootBallCommand.java | 6 +- .../commands/shooter/FeedBallCommand.java | 10 --- .../pigmice/frc/robot/subsystems/Shooter.java | 56 +++++++------ 5 files changed, 88 insertions(+), 83 deletions(-) rename src/main/java/com/pigmice/frc/robot/commands/{shooter => }/ShootBallCommand.java (82%) delete mode 100644 src/main/java/com/pigmice/frc/robot/commands/shooter/FeedBallCommand.java diff --git a/src/main/java/com/pigmice/frc/robot/Constants.java b/src/main/java/com/pigmice/frc/robot/Constants.java index ce992f1..1c14558 100644 --- a/src/main/java/com/pigmice/frc/robot/Constants.java +++ b/src/main/java/com/pigmice/frc/robot/Constants.java @@ -29,7 +29,26 @@ public static class ShooterConfig { public static final double shooterS = 0; public static final double shooterV = .5; - public static final double velocityThreshhold = 100; // tune this + public static final double velocityThreshold = 100; // tune this + + public static enum ShooterModes { + AUTO(-1, -1), FENDER_LOW(0, 0), FENDER_HIGH(900, 2400), TARMAC(1600, 1800), LAUNCHPAD(0, 0); + + private double topRPM, bottomRPM; + + private ShooterModes(double topRPM, double botRPM) { + this.topRPM = topRPM; + this.bottomRPM = botRPM; + } + + public double getTopRPM() { + return topRPM; + } + + public double getBottomRPM() { + return bottomRPM; + } + } } public static class ClimberConfig { diff --git a/src/main/java/com/pigmice/frc/robot/RobotContainer.java b/src/main/java/com/pigmice/frc/robot/RobotContainer.java index fd9e137..b0f1796 100644 --- a/src/main/java/com/pigmice/frc/robot/RobotContainer.java +++ b/src/main/java/com/pigmice/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import java.util.List; import com.pigmice.frc.robot.Constants.DrivetrainConfig; +import com.pigmice.frc.robot.commands.ShootBallCommand; import com.pigmice.frc.robot.commands.Indexer.SpinIndexerToAngle; import com.pigmice.frc.robot.commands.Indexer.SpinIndexerToAngleOld; import com.pigmice.frc.robot.commands.climber.ClimbRung; @@ -15,7 +16,6 @@ import com.pigmice.frc.robot.commands.drivetrain.DriveDistance; import com.pigmice.frc.robot.commands.intake.ExtendIntake; import com.pigmice.frc.robot.commands.intake.RetractIntake; -import com.pigmice.frc.robot.commands.shooter.ShootBallCommand; import com.pigmice.frc.robot.subsystems.Drivetrain; import com.pigmice.frc.robot.subsystems.Indexer; import com.pigmice.frc.robot.subsystems.Intake; @@ -114,7 +114,7 @@ private void configureButtonBindings(XboxController driver, XboxController opera // OPERATOR CONTROLS new JoystickButton(operator, Button.kLeftStick.value) - .whenPressed(() -> this.shootMode = !shootMode); + .whenPressed(() -> this.shootMode = !shootMode); // TODO Create target variables for both rotato and lifty that the default // commands will use @@ -122,55 +122,57 @@ private void configureButtonBindings(XboxController driver, XboxController opera // for both default commands new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kRightBumper.value).get()) - .whenActive(() -> this.liftOutput = 0.30) - .whenInactive(() -> this.liftOutput = 0.00); + new JoystickButton(operator, Button.kRightBumper.value).get()) + .whenActive(() -> this.liftOutput = 0.30) + .whenInactive(() -> this.liftOutput = 0.00); new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kLeftBumper.value).get()) - .whenActive(() -> this.liftOutput = -0.30) - .whenInactive(() -> this.liftOutput = 0.00); + new JoystickButton(operator, Button.kLeftBumper.value).get()) + .whenActive(() -> this.liftOutput = -0.30) + .whenInactive(() -> this.liftOutput = 0.00); new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kA.value).get()) - .whenActive(() -> this.rotateOutput = 0.35) - .whenInactive(() -> this.rotateOutput = 0.00); + new JoystickButton(operator, Button.kA.value).get()) + .whenActive(() -> this.rotateOutput = 0.35) + .whenInactive(() -> this.rotateOutput = 0.00); new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kB.value).get()) - .whenActive(() -> this.rotateOutput = -0.35) - .whenInactive(() -> this.rotateOutput = 0.00); + new JoystickButton(operator, Button.kB.value).get()) + .whenActive(() -> this.rotateOutput = -0.35) + .whenInactive(() -> this.rotateOutput = 0.00); new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kX.value).get()) - .whenActive(() -> this.rotateOutput = 0.15) - .whenInactive(() -> this.rotateOutput = 0.00); - + new JoystickButton(operator, Button.kX.value).get()) + .whenActive(() -> this.rotateOutput = 0.15) + .whenInactive(() -> this.rotateOutput = 0.00); + new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kY.value).get()) - .whenActive(() -> this.rotateOutput = -0.15) - .whenInactive(() -> this.rotateOutput = 0.00); - + new JoystickButton(operator, Button.kY.value).get()) + .whenActive(() -> this.rotateOutput = -0.15) + .whenInactive(() -> this.rotateOutput = 0.00); + new Trigger(() -> shootMode == false && - new JoystickButton(operator, Button.kRightStick.value).get()) - .whenActive(new ClimbRung(lifty, rotato)); + new JoystickButton(operator, Button.kRightStick.value).get()) + .whenActive(new ClimbRung(lifty, rotato)); - /* new Trigger(() -> mode == false && - new JoystickButton(operator, Button.kBack.value).get()) - .whenActive(new ClimbMid(lifty, rotato)); */ + /* + * new Trigger(() -> mode == false && + * new JoystickButton(operator, Button.kBack.value).get()) + * .whenActive(new ClimbMid(lifty, rotato)); + */ + + new Trigger(() -> shootMode == true && + new JoystickButton(operator, Button.kA.value).get()) + .whenActive(new ExtendIntake(intake)) + .whenInactive(new RetractIntake(intake)); new Trigger(() -> shootMode == true && - new JoystickButton(operator, Button.kA.value).get()) - .whenActive(new ExtendIntake(intake)) - .whenInactive(new RetractIntake(intake)); - - new Trigger(() -> shootMode == true && - operator.getRightTriggerAxis() >= (1 - DrivetrainConfig.driveThreshold)) - .whenActive(new ShootBallCommand(shooter, indexer)) - .whenInactive(() -> { - this.shooter.disable(); - this.indexer.disable(); - }); + operator.getRightTriggerAxis() >= (1 - DrivetrainConfig.driveThreshold)) + .whenActive(new ShootBallCommand(shooter, indexer)) + .whenInactive(() -> { + this.shooter.disable(); + this.indexer.disable(); + }); } // private double getPower() { diff --git a/src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java b/src/main/java/com/pigmice/frc/robot/commands/ShootBallCommand.java similarity index 82% rename from src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java rename to src/main/java/com/pigmice/frc/robot/commands/ShootBallCommand.java index c96132d..c843229 100644 --- a/src/main/java/com/pigmice/frc/robot/commands/shooter/ShootBallCommand.java +++ b/src/main/java/com/pigmice/frc/robot/commands/ShootBallCommand.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package com.pigmice.frc.robot.commands.shooter; +package com.pigmice.frc.robot.commands; import com.pigmice.frc.robot.commands.Indexer.SpinIndexerToAngle; import com.pigmice.frc.robot.subsystems.Indexer; @@ -13,13 +13,11 @@ public class ShootBallCommand extends SequentialCommandGroup { public ShootBallCommand(Shooter shooter, Indexer indexer) { super( - new InstantCommand(() -> shooter.enable()), new WaitUntilCommand(shooter::isAtTargetVelocity), new InstantCommand(() -> indexer.resetEncoder()), new InstantCommand(() -> indexer.enable()), new SpinIndexerToAngle(indexer, 200, false)); - addRequirements(shooter); - addRequirements(indexer); + addRequirements(shooter, indexer); } } \ No newline at end of file diff --git a/src/main/java/com/pigmice/frc/robot/commands/shooter/FeedBallCommand.java b/src/main/java/com/pigmice/frc/robot/commands/shooter/FeedBallCommand.java deleted file mode 100644 index 1b17b20..0000000 --- a/src/main/java/com/pigmice/frc/robot/commands/shooter/FeedBallCommand.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.pigmice.frc.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class FeedBallCommand extends CommandBase { - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java index 0f4dc07..dc506ec 100644 --- a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java +++ b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.pigmice.frc.robot.Constants.ShooterConfig; +import com.pigmice.frc.robot.Constants.ShooterConfig.ShooterModes; import com.pigmice.frc.robot.RPMPController; import com.pigmice.frc.robot.Utils; @@ -16,26 +17,17 @@ public class Shooter extends SubsystemBase { private boolean enabled = false; - private TalonSRX topMotor; - private TalonSRX botMotor; + private TalonSRX topMotor, botMotor; - private double shooterP, shooterS, shooterV, vThresh; + private double shooterP, vThresh; private final RPMPController topController = new RPMPController(shooterP, 0.25); private final RPMPController botController = new RPMPController(shooterP, 0.25); private final ShuffleboardTab shooterTab; - private final NetworkTableEntry topRPMEntry; - private final NetworkTableEntry bottomRPMEntry; - - private final NetworkTableEntry actualTopRPM; - private final NetworkTableEntry actualBottomRPM; - - private final NetworkTableEntry topCalculated; - private final NetworkTableEntry botCalculated; - - private final NetworkTableEntry atTargetEntry; + private final NetworkTableEntry topRPMEntry, bottomRPMEntry, actualTopRPM, + actualBottomRPM, topCalculated, botCalculated, atTargetEntry; private final FeedbackDevice feedbackDevice = FeedbackDevice.CTRE_MagEncoder_Absolute; @@ -49,6 +41,8 @@ public class Shooter extends SubsystemBase { private boolean atTarget = false; + private ShooterModes mode; + // Create a new Shooter public Shooter() { this.topMotor = new TalonSRX(ShooterConfig.topMotorPort); @@ -66,9 +60,7 @@ public Shooter() { // botMotor.setNeutralMode(NeutralMode.Coast); this.shooterP = ShooterConfig.shooterP; - this.shooterS = ShooterConfig.shooterS; - this.shooterV = ShooterConfig.shooterV; - this.vThresh = ShooterConfig.velocityThreshhold; + this.vThresh = ShooterConfig.velocityThreshold; this.shooterTab = Shuffleboard.getTab("Shooter"); @@ -82,6 +74,8 @@ public Shooter() { this.botCalculated = shooterTab.add("Bottom Calculated Velocity", 1).getEntry(); this.atTargetEntry = shooterTab.add("At Target", this.atTarget).getEntry(); + + this.mode = ShooterModes.AUTO; } public void enable() { @@ -105,20 +99,18 @@ public void periodic() { if (!enabled) { this.setTargetSpeeds(0, 0); } else { - this.setTargetSpeeds(2400, 2000); + if (mode == ShooterModes.AUTO) { + // calculate speeds based on distance + } else { + this.setTargetSpeeds(mode.getTopRPM(), mode.getBottomRPM()); + } } - // this.setTargetSpeeds(2600, 2300); - // this.setTargetSpeeds(2000, 1750); - double topRPM = this.topTargetRPM == RPM_NOT_SET ? this.topRPMEntry.getDouble(ShooterConfig.topMotorSpeed) : this.topTargetRPM; double botRPM = this.botTargetRPM == RPM_NOT_SET ? this.bottomRPMEntry.getDouble(ShooterConfig.bottomMotorSpeed) : this.botTargetRPM; - // from tarmac edge - // this.setTargetSpeeds(1600, 1800); - // from fender - // this.setTargetSpeeds(900, 2400); + double topVelocity = topMotor.getSelectedSensorVelocity(); double botVelocity = botMotor.getSelectedSensorVelocity(); @@ -146,6 +138,11 @@ public void periodic() { botMotor.set(ControlMode.PercentOutput, botTarget); } + @Override + public void simulationPeriodic() { + this.periodic(); + } + /** * Sets the target RPMs of the top and bottom shooter motors. * @@ -171,19 +168,18 @@ public void stopMotors() { this.topTargetRPM = this.botTargetRPM = 0; this.topController.setTargetRPM(0); this.botController.setTargetRPM(0); - this.atTarget = false; + this.atTarget = false; // explain why this is necessary } public boolean isAtTargetVelocity() { return topTargetRPM != 0 && botTargetRPM != 0 && this.atTarget; } - private double calculate(double velocity) { - return shooterS * Math.signum(velocity) + shooterV * velocity; + public ShooterModes getMode() { + return mode; } - @Override - public void simulationPeriodic() { - periodic(); + public void setMode(ShooterModes mode) { + this.mode = mode; } } \ No newline at end of file From 5f699f200746f7c93ece06643276c21e4afe3443 Mon Sep 17 00:00:00 2001 From: ev118 Date: Wed, 23 Mar 2022 14:27:57 -0700 Subject: [PATCH 4/7] fixed shooter methods to enable/disable properly --- src/main/java/com/pigmice/frc/robot/RobotContainer.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/pigmice/frc/robot/RobotContainer.java b/src/main/java/com/pigmice/frc/robot/RobotContainer.java index 2de1cfd..4d69a3a 100644 --- a/src/main/java/com/pigmice/frc/robot/RobotContainer.java +++ b/src/main/java/com/pigmice/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import com.pigmice.frc.robot.commands.intake.RetractIntake; import com.pigmice.frc.robot.commands.shooter.StartShooterCommand; import com.pigmice.frc.robot.subsystems.Drivetrain; +import com.pigmice.frc.robot.subsystems.Subsystem; import com.pigmice.frc.robot.subsystems.Indexer; import com.pigmice.frc.robot.subsystems.Intake; import com.pigmice.frc.robot.subsystems.Shooter; @@ -122,7 +123,7 @@ public RobotContainer() { * it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings(XboxController driver, XboxController operator) { - + // DRIVER CONTROLS new JoystickButton(driver, Button.kY.value) @@ -257,13 +258,11 @@ public void onInit() { public void onEnable() { this.shooter.setMode(ShooterMode.OFF); - this.shooter.enable(); this.indexer.enable(); this.intake.enable(); } public void onDisable() { - this.shooter.setMode(ShooterMode.OFF); CommandScheduler.getInstance().cancelAll(); this.shooter.disable(); this.indexer.disable(); From 85b848c75fb966214cc5c59a397ed226a5f13d14 Mon Sep 17 00:00:00 2001 From: ev118 Date: Wed, 23 Mar 2022 16:09:05 -0700 Subject: [PATCH 5/7] added shooter speeds by distance Also implementation in the subsystem. Actual speeds still need to be tested. --- .../pigmice/frc/robot/ShooterDistances.java | 43 +++++++++++++++++++ .../commands/shooter/StartShooterCommand.java | 4 -- .../pigmice/frc/robot/subsystems/Shooter.java | 9 ++++ 3 files changed, 52 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/pigmice/frc/robot/ShooterDistances.java diff --git a/src/main/java/com/pigmice/frc/robot/ShooterDistances.java b/src/main/java/com/pigmice/frc/robot/ShooterDistances.java new file mode 100644 index 0000000..ed8e530 --- /dev/null +++ b/src/main/java/com/pigmice/frc/robot/ShooterDistances.java @@ -0,0 +1,43 @@ +package com.pigmice.frc.robot; + +public class ShooterDistances { + private static int[] topSpeeds = {}; + private static int[] bottomSpeeds = {}; + private static int[] distances = new int[31]; { // distances in inches + for(int a = 0; a < 31; a++) { + distances[a] = a*6; + }} + + private static int find(int[] a, double i) { + int k = (int) i; + for(int j = 0; j < a.length; j++) { + if(a[j] == k) {return j;} + } + return -1; + } + + public static double getTopSpeed(double distance) { + if(distance % 6 == 0) { + return topSpeeds[find(distances, distance)]; + } else { + double remainder = distance % 6; + int lowerBound = topSpeeds[find(distances, distance - remainder)]; + int upperBound = topSpeeds[find(distances, distance + 6 - remainder)]; + return ((lowerBound * (6 - remainder)) + (upperBound * remainder)) / 6; + } + } + + public static double getBottomSpeed(double distance) { + if(distance % 6 == 0) { + return bottomSpeeds[find(distances, distance)]; + } else { + double remainder = distance % 6; + int lowerBound = bottomSpeeds[find(distances, distance - remainder)]; + int upperBound = bottomSpeeds[find(distances, distance + 6 - remainder)]; + return ((lowerBound * (6 - remainder)) + (upperBound * remainder)) / 6; + } + } + + public static int[] listTopSpeeds() {return topSpeeds;} + public static int[] listBottomSpeeds() {return bottomSpeeds;} +} \ No newline at end of file diff --git a/src/main/java/com/pigmice/frc/robot/commands/shooter/StartShooterCommand.java b/src/main/java/com/pigmice/frc/robot/commands/shooter/StartShooterCommand.java index d7602e3..c8b53a0 100644 --- a/src/main/java/com/pigmice/frc/robot/commands/shooter/StartShooterCommand.java +++ b/src/main/java/com/pigmice/frc/robot/commands/shooter/StartShooterCommand.java @@ -21,10 +21,6 @@ public void initialize() { this.shooter.setMode(mode); } - @Override - public void execute() { - } - @Override public boolean isFinished() { return this.shooter.isAtTargetVelocity(); diff --git a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java index ae7e805..4424d09 100644 --- a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java +++ b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java @@ -1,5 +1,7 @@ package com.pigmice.frc.robot.subsystems; +import com.pigmice.frc.robot.ShooterDistances; +import com.pigmice.frc.robot.Vision; import com.pigmice.frc.robot.Constants.ShooterConfig; import com.pigmice.frc.robot.Constants.ShooterConfig.ShooterMode; import com.revrobotics.CANSparkMax; @@ -25,6 +27,7 @@ public class Shooter extends Subsystem { private final RelativeEncoder topEncoder, botEncoder; public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM, maxVel, minVel, maxAcc, allowedErr; + private double hubDistance; private final ShuffleboardTab shooterTab; @@ -128,6 +131,12 @@ public void periodic() { double topRPM = this.mode.getTopRPM(); double botRPM = this.mode.getBottomRPM(); + if(this.mode == ShooterMode.AUTO){ + hubDistance = Vision.getDistanceFromTarget(Vision.getBestTarget()); + topRPM = ShooterDistances.getTopSpeed(hubDistance); + botRPM = ShooterDistances.getBottomSpeed(hubDistance); + } + if (this.mode == ShooterMode.SHUFFLEBOARD || this.isTestMode()) { topRPM = topRPMEntry.getDouble(topRPM); botRPM = bottomRPMEntry.getDouble(botRPM); From fb132f22ed23f51838cd02e6b238d3bb3b263cc8 Mon Sep 17 00:00:00 2001 From: Alex Date: Thu, 24 Mar 2022 16:26:19 -0700 Subject: [PATCH 6/7] updated ShooterDistances for fewer data points --- .../pigmice/frc/robot/ShooterDistances.java | 90 ++++++++++++------- .../pigmice/frc/robot/subsystems/Shooter.java | 4 +- 2 files changed, 61 insertions(+), 33 deletions(-) diff --git a/src/main/java/com/pigmice/frc/robot/ShooterDistances.java b/src/main/java/com/pigmice/frc/robot/ShooterDistances.java index ed8e530..6a7cd5b 100644 --- a/src/main/java/com/pigmice/frc/robot/ShooterDistances.java +++ b/src/main/java/com/pigmice/frc/robot/ShooterDistances.java @@ -1,43 +1,71 @@ package com.pigmice.frc.robot; +import java.util.List; + +import edu.wpi.first.math.MathUtil; + public class ShooterDistances { - private static int[] topSpeeds = {}; - private static int[] bottomSpeeds = {}; - private static int[] distances = new int[31]; { // distances in inches - for(int a = 0; a < 31; a++) { - distances[a] = a*6; - }} - - private static int find(int[] a, double i) { - int k = (int) i; - for(int j = 0; j < a.length; j++) { - if(a[j] == k) {return j;} + public static class ShooterPoint { + public double distance, topSpeed, bottomSpeed; + + ShooterPoint(double dist, double tSpeed, double bSpeed){ + distance = dist; + topSpeed = tSpeed; + bottomSpeed = bSpeed; } - return -1; + + public double getDistance() { + return distance; + } + + public void setDistance(double distance) { + this.distance = distance; + } + + public double getTopSpeed() { + return topSpeed; + } + + public void setTopSpeed(double topSpeed) { + this.topSpeed = topSpeed; + } + + public double getBottomSpeed() { + return bottomSpeed; + } + + public void setBottomSpeed(double bottomSpeed) { + this.bottomSpeed = bottomSpeed; + } } - public static double getTopSpeed(double distance) { - if(distance % 6 == 0) { - return topSpeeds[find(distances, distance)]; - } else { - double remainder = distance % 6; - int lowerBound = topSpeeds[find(distances, distance - remainder)]; - int upperBound = topSpeeds[find(distances, distance + 6 - remainder)]; - return ((lowerBound * (6 - remainder)) + (upperBound * remainder)) / 6; + private static final List pointList = List.of(new ShooterPoint(0,0,0), new ShooterPoint(0,0,0)); + + private static ShooterPoint calcSpeeds (double distance) { + ShooterPoint speeds = new ShooterPoint(distance, 0, 0); + ShooterPoint lowerBound, upperBound; + lowerBound = upperBound = speeds; + + for(ShooterPoint i : pointList){ + if(i.getDistance() > distance){ + upperBound = i; + lowerBound = pointList.get(pointList.indexOf(i) - 1); + break; + } } + + double betweenPortion = (distance - lowerBound.getDistance()) / (upperBound.getDistance() - lowerBound.getDistance()); + speeds.setTopSpeed(MathUtil.interpolate(lowerBound.getTopSpeed(), upperBound.getTopSpeed(), betweenPortion)); + speeds.setBottomSpeed(MathUtil.interpolate(lowerBound.getBottomSpeed(), upperBound.getBottomSpeed(), betweenPortion)); + + return speeds; } - public static double getBottomSpeed(double distance) { - if(distance % 6 == 0) { - return bottomSpeeds[find(distances, distance)]; - } else { - double remainder = distance % 6; - int lowerBound = bottomSpeeds[find(distances, distance - remainder)]; - int upperBound = bottomSpeeds[find(distances, distance + 6 - remainder)]; - return ((lowerBound * (6 - remainder)) + (upperBound * remainder)) / 6; - } + public static double findTopSpeed (double distance) { + return calcSpeeds(distance).getTopSpeed(); } - public static int[] listTopSpeeds() {return topSpeeds;} - public static int[] listBottomSpeeds() {return bottomSpeeds;} + public static double findBottomSpeed (double distance) { + return calcSpeeds(distance).getBottomSpeed(); + } } \ No newline at end of file diff --git a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java index 4424d09..544065e 100644 --- a/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java +++ b/src/main/java/com/pigmice/frc/robot/subsystems/Shooter.java @@ -133,8 +133,8 @@ public void periodic() { if(this.mode == ShooterMode.AUTO){ hubDistance = Vision.getDistanceFromTarget(Vision.getBestTarget()); - topRPM = ShooterDistances.getTopSpeed(hubDistance); - botRPM = ShooterDistances.getBottomSpeed(hubDistance); + topRPM = ShooterDistances.findTopSpeed(hubDistance); + botRPM = ShooterDistances.findBottomSpeed(hubDistance); } if (this.mode == ShooterMode.SHUFFLEBOARD || this.isTestMode()) { From 0b8ce16deb505bda222ed5995b6aa334fead7ab9 Mon Sep 17 00:00:00 2001 From: Alex Date: Thu, 24 Mar 2022 16:30:46 -0700 Subject: [PATCH 7/7] RetractIntake-indexer errors --- src/main/java/com/pigmice/frc/robot/RobotContainer.java | 2 +- .../pigmice/frc/robot/commands/drivetrain/Auto2BallFender.java | 2 +- src/main/java/com/pigmice/frc/robot/subsystems/Intake.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/pigmice/frc/robot/RobotContainer.java b/src/main/java/com/pigmice/frc/robot/RobotContainer.java index 6777b75..9074e8b 100644 --- a/src/main/java/com/pigmice/frc/robot/RobotContainer.java +++ b/src/main/java/com/pigmice/frc/robot/RobotContainer.java @@ -147,7 +147,7 @@ private void configureButtonBindings(XboxController driver, XboxController opera .whenActive( new ExtendIntake(intake)) .whenInactive( - new RetractIntake(intake)); + new RetractIntake(intake, indexer)); // [operator] manually eject all balls new Trigger(() -> shootMode == true && diff --git a/src/main/java/com/pigmice/frc/robot/commands/drivetrain/Auto2BallFender.java b/src/main/java/com/pigmice/frc/robot/commands/drivetrain/Auto2BallFender.java index f23daec..442ef43 100644 --- a/src/main/java/com/pigmice/frc/robot/commands/drivetrain/Auto2BallFender.java +++ b/src/main/java/com/pigmice/frc/robot/commands/drivetrain/Auto2BallFender.java @@ -15,7 +15,7 @@ public class Auto2BallFender extends SequentialCommandGroup { public Auto2BallFender(Indexer indexer, Shooter shooter, Intake intake, Drivetrain drivetrain) { super(new ExtendIntake(intake), new DriveDistance(drivetrain, 1.0), - new RetractIntake(intake), + new RetractIntake(intake, indexer), new DriveDistance(drivetrain, -2.5), new ShootBallWithModeCommand(indexer, shooter, intake, ShooterMode.FENDER_HIGH)); } diff --git a/src/main/java/com/pigmice/frc/robot/subsystems/Intake.java b/src/main/java/com/pigmice/frc/robot/subsystems/Intake.java index 35cbe99..646e459 100644 --- a/src/main/java/com/pigmice/frc/robot/subsystems/Intake.java +++ b/src/main/java/com/pigmice/frc/robot/subsystems/Intake.java @@ -260,7 +260,7 @@ public void testPeriodic() { if (!extended) { CommandScheduler.getInstance().schedule(new ExtendIntake(this)); } else if (extended) { - CommandScheduler.getInstance().schedule(new RetractIntake(this)); + CommandScheduler.getInstance().schedule(new RetractIntake(this, null)); } System.out.println("Value Toggled");