diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a42f0bbc1..63b12d96a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.magicCarpet.MagicCarpetIO; import frc.robot.subsystems.magicCarpet.MagicCarpetSparkMax; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSparkMax; import frc.robot.subsystems.vision.Vision; @@ -149,6 +150,7 @@ public RobotContainer() { // leds = new Leds(); // hopperBelt = new HopperBelt(new HopperBeltSparkMax()); leds = new Leds(); + shooter = new Shooter(new ShooterIOSparkMax(true)); } } } @@ -385,6 +387,58 @@ private void configureButtonBindings() { () -> operatorController.getLeftY() * Units.rotationsPerMinuteToRadiansPerSecond(5600))); + + if (Constants.getRobot() == Constants.RobotType.ROBOT_BRIEFCASE) { + // All 4 motors at max: A (forward) / back+A (reverse) + driverController + .a() + .and(() -> !driverController.back().getAsBoolean()) + .whileTrue(shooter.setVoltage(ShooterConstants.MAX_VOLTAGE)); + driverController + .a() + .and(driverController.back()) + .whileTrue(shooter.setVoltage(-ShooterConstants.MAX_VOLTAGE)); + + // Bottom-left: B (forward) / back+B (reverse) + driverController + .b() + .and(() -> !driverController.back().getAsBoolean()) + .whileTrue(shooter.setBottomLeftVoltage(ShooterConstants.MAX_VOLTAGE)); + driverController + .b() + .and(driverController.back()) + .whileTrue(shooter.setBottomLeftVoltage(-ShooterConstants.MAX_VOLTAGE)); + + // Top-left: X (forward) / back+X (reverse) + driverController + .x() + .and(() -> !driverController.back().getAsBoolean()) + .whileTrue(shooter.setTopLeftVoltage(ShooterConstants.MAX_VOLTAGE)); + driverController + .x() + .and(driverController.back()) + .whileTrue(shooter.setTopLeftVoltage(-ShooterConstants.MAX_VOLTAGE)); + + // Top-right: Y (forward) / back+Y (reverse) + driverController + .y() + .and(() -> !driverController.back().getAsBoolean()) + .whileTrue(shooter.setTopRightVoltage(ShooterConstants.MAX_VOLTAGE)); + driverController + .y() + .and(driverController.back()) + .whileTrue(shooter.setTopRightVoltage(-ShooterConstants.MAX_VOLTAGE)); + + // Bottom-right: left bumper (forward) / back+left bumper (reverse) + driverController + .leftBumper() + .and(() -> !driverController.back().getAsBoolean()) + .whileTrue(shooter.setBottomRightVoltage(ShooterConstants.MAX_VOLTAGE)); + driverController + .leftBumper() + .and(driverController.back()) + .whileTrue(shooter.setBottomRightVoltage(-ShooterConstants.MAX_VOLTAGE)); + } } /** diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java index 38d064911..1065428f2 100644 --- a/src/main/java/frc/robot/Schematic.java +++ b/src/main/java/frc/robot/Schematic.java @@ -20,9 +20,10 @@ public class Schematic { public static final int backRightAbsoluteEncoderCanId; // shooter subsystem - public static final int shooterBottomMotorCanId; - public static final int shooterTopMotorCanId; - public static final int shooterMiddleMotorCanId; + public static final int shooterTopLeftMotorCanId; + public static final int shooterTopRightMotorCanId; + public static final int shooterBottomLeftMotorCanId; + public static final int shooterBottomRightMotorCanId; // magic carpet sub-system public static final int magicCarpetCanId; @@ -60,9 +61,10 @@ public class Schematic { backLeftAbsoluteEncoderCanId = 20; // Shooter (CAN Ids) - shooterBottomMotorCanId = 0; - shooterTopMotorCanId = 0; - shooterMiddleMotorCanId = 0; + shooterTopLeftMotorCanId = 0; + shooterTopRightMotorCanId = 0; + shooterBottomLeftMotorCanId = 0; + shooterBottomRightMotorCanId = 0; // Magic Carpet (CAN Ids) magicCarpetCanId = 0; @@ -99,9 +101,10 @@ public class Schematic { backLeftAbsoluteEncoderCanId = 21; // Shooter (CAN Ids) - shooterTopMotorCanId = 11; - shooterMiddleMotorCanId = 12; - shooterBottomMotorCanId = 13; + shooterTopLeftMotorCanId = 11; + shooterTopRightMotorCanId = 12; + shooterBottomLeftMotorCanId = 13; + shooterBottomRightMotorCanId = 14; // Hopper (CAN Ids) magicCarpetCanId = 15; @@ -137,9 +140,10 @@ public class Schematic { backRightAbsoluteEncoderCanId = 0; // Shooter (CAN Ids) - shooterBottomMotorCanId = 0; - shooterTopMotorCanId = 0; - shooterMiddleMotorCanId = 0; + shooterTopLeftMotorCanId = 0; + shooterTopRightMotorCanId = 0; + shooterBottomLeftMotorCanId = 0; + shooterBottomRightMotorCanId = 0; // Magic Carpet (CAN Ids) magicCarpetCanId = 0; @@ -175,9 +179,10 @@ public class Schematic { backRightAbsoluteEncoderCanId = 0; // Shooter (CAN Ids) - shooterBottomMotorCanId = 0; - shooterTopMotorCanId = 0; - shooterMiddleMotorCanId = 0; + shooterTopLeftMotorCanId = 0; + shooterTopRightMotorCanId = 0; + shooterBottomLeftMotorCanId = 0; + shooterBottomRightMotorCanId = 0; // Magic Carpet (CAN Ids) magicCarpetCanId = 0; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b9aac7cf9..e9e0a469d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -112,6 +112,30 @@ public Command setVoltage(double volts) { Commands.run(() -> io.setVoltage(volts), this)); } + public Command setTopLeftVoltage(double volts) { + return Commands.sequence( + Commands.runOnce(() -> controllerEnabled = false, this), + Commands.run(() -> io.setTopLeftVoltage(volts), this)); + } + + public Command setTopRightVoltage(double volts) { + return Commands.sequence( + Commands.runOnce(() -> controllerEnabled = false, this), + Commands.run(() -> io.setTopRightVoltage(volts), this)); + } + + public Command setBottomLeftVoltage(double volts) { + return Commands.sequence( + Commands.runOnce(() -> controllerEnabled = false, this), + Commands.run(() -> io.setBottomLeftVoltage(volts), this)); + } + + public Command setBottomRightVoltage(double volts) { + return Commands.sequence( + Commands.runOnce(() -> controllerEnabled = false, this), + Commands.run(() -> io.setBottomRightVoltage(volts), this)); + } + public Command setTargetVelocityRPM(double rpm) { return Commands.run( () -> { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 03f011667..612648e50 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -6,17 +6,21 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { - public double middleMotorVelocityRadPerSec; - public double middleMotorAppliedVolts; - public double middleMotorCurrentAmps; + public double topLeftMotorVelocityRadPerSec; + public double topLeftMotorAppliedVolts; + public double topLeftMotorCurrentAmps; - public double bottomMotorVelocityRadPerSec; - public double bottomMotorAppliedVolts; - public double bottomMotorCurrentAmps; + public double topRightMotorVelocityRadPerSec; + public double topRightMotorAppliedVolts; + public double topRightMotorCurrentAmps; - public double topMotorVelocityRadPerSec; - public double topMotorAppliedVolts; - public double topMotorCurrentAmps; + public double bottomLeftMotorVelocityRadPerSec; + public double bottomLeftMotorAppliedVolts; + public double bottomLeftMotorCurrentAmps; + + public double bottomRightMotorVelocityRadPerSec; + public double bottomRightMotorAppliedVolts; + public double bottomRightMotorCurrentAmps; public boolean atSetpoint = false; public double setpointRPM = 0; @@ -31,5 +35,13 @@ default void updateInputs(ShooterIOInputs inputs) {} default void setVoltage(double volts) {} + default void setTopLeftVoltage(double volts) {} + + default void setTopRightVoltage(double volts) {} + + default void setBottomLeftVoltage(double volts) {} + + default void setBottomRightVoltage(double volts) {} + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index bb97cfc34..1b9f090ec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.shooter; -import static frc.robot.Schematic.shooterBottomMotorCanId; -import static frc.robot.Schematic.shooterMiddleMotorCanId; -import static frc.robot.Schematic.shooterTopMotorCanId; +import static frc.robot.Schematic.shooterBottomLeftMotorCanId; +import static frc.robot.Schematic.shooterBottomRightMotorCanId; +import static frc.robot.Schematic.shooterTopLeftMotorCanId; +import static frc.robot.Schematic.shooterTopRightMotorCanId; import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; @@ -15,94 +16,148 @@ public class ShooterIOSparkMax implements ShooterIO { - private final SparkMax middleMotor; - private final SparkMax bottomMotor; - private final SparkMax topMotor; - private final RelativeEncoder middleMotorEncoder; - private final RelativeEncoder bottomMotorEncoder; - private final RelativeEncoder topMotorEncoder; + private final SparkMax topLeftMotor; + private final SparkMax topRightMotor; + private final SparkMax bottomLeftMotor; + private final SparkMax bottomRightMotor; + private final RelativeEncoder topLeftMotorEncoder; + private final RelativeEncoder topRightMotorEncoder; + private final RelativeEncoder bottomLeftMotorEncoder; + private final RelativeEncoder bottomRightMotorEncoder; + private final boolean independentMode; public ShooterIOSparkMax() { - middleMotor = new SparkMax(shooterMiddleMotorCanId, MotorType.kBrushless); - bottomMotor = new SparkMax(shooterBottomMotorCanId, MotorType.kBrushless); - topMotor = new SparkMax(shooterTopMotorCanId, MotorType.kBrushless); + this(false); + } + + public ShooterIOSparkMax(boolean independentMode) { + this.independentMode = independentMode; + topLeftMotor = new SparkMax(shooterTopLeftMotorCanId, MotorType.kBrushless); + topRightMotor = new SparkMax(shooterTopRightMotorCanId, MotorType.kBrushless); + bottomLeftMotor = new SparkMax(shooterBottomLeftMotorCanId, MotorType.kBrushless); + bottomRightMotor = new SparkMax(shooterBottomRightMotorCanId, MotorType.kBrushless); + + EncoderConfig enc = new EncoderConfig(); + enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); + enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); - var middleMotorConfig = new SparkMaxConfig(); - middleMotorConfig + var bottomLeftMotorConfig = new SparkMaxConfig(); + bottomLeftMotorConfig .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) .smartCurrentLimit(CURRENT_LIMIT); + bottomLeftMotorConfig.apply(enc); - var bottomMotorConfig = new SparkMaxConfig(); - bottomMotorConfig - .inverted(true) + var topLeftMotorConfig = new SparkMaxConfig(); + topLeftMotorConfig + .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) .smartCurrentLimit(CURRENT_LIMIT); + if (!independentMode) topLeftMotorConfig.follow(bottomLeftMotor.getDeviceId(), false); + topLeftMotorConfig.apply(enc); - var topMotorConfig = new SparkMaxConfig(); - topMotorConfig - .inverted(false) + var bottomRightMotorConfig = new SparkMaxConfig(); + bottomRightMotorConfig + .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) .smartCurrentLimit(CURRENT_LIMIT); + if (!independentMode) bottomRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true); + bottomRightMotorConfig.apply(enc); - topMotorConfig.follow(bottomMotor.getDeviceId(), false); - middleMotorConfig.follow(bottomMotor.getDeviceId(), true); - - EncoderConfig enc = new EncoderConfig(); - enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); - enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); - middleMotorConfig.apply(enc); - bottomMotorConfig.apply(enc); - topMotorConfig.apply(enc); - - middleMotor.configure( - middleMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - bottomMotor.configure( - bottomMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - topMotor.configure( - topMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - middleMotorEncoder = middleMotor.getEncoder(); - bottomMotorEncoder = bottomMotor.getEncoder(); - topMotorEncoder = topMotor.getEncoder(); + var topRightMotorConfig = new SparkMaxConfig(); + topRightMotorConfig + .inverted(true) + .idleMode(IDLE_MODE) + .voltageCompensation(VOLTAGE_COMPENSATION) + .smartCurrentLimit(CURRENT_LIMIT); + if (!independentMode) topRightMotorConfig.follow(bottomLeftMotor.getDeviceId(), true); + topRightMotorConfig.apply(enc); + + bottomLeftMotor.configure( + bottomLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + topLeftMotor.configure( + topLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + bottomRightMotor.configure( + bottomRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + topRightMotor.configure( + topRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + topLeftMotorEncoder = topLeftMotor.getEncoder(); + topRightMotorEncoder = topRightMotor.getEncoder(); + bottomLeftMotorEncoder = bottomLeftMotor.getEncoder(); + bottomRightMotorEncoder = bottomRightMotor.getEncoder(); } @Override public void updateInputs(ShooterIOInputs inputs) { - inputs.middleMotorCurrentAmps = middleMotor.getOutputCurrent(); - inputs.middleMotorAppliedVolts = middleMotor.getBusVoltage() * middleMotor.getAppliedOutput(); - inputs.middleMotorVelocityRadPerSec = middleMotorEncoder.getVelocity(); - - inputs.bottomMotorCurrentAmps = bottomMotor.getOutputCurrent(); - inputs.bottomMotorAppliedVolts = bottomMotor.getBusVoltage() * bottomMotor.getAppliedOutput(); - inputs.bottomMotorVelocityRadPerSec = bottomMotorEncoder.getVelocity(); - - inputs.topMotorCurrentAmps = topMotor.getOutputCurrent(); - inputs.topMotorAppliedVolts = topMotor.getBusVoltage() * topMotor.getAppliedOutput(); - inputs.topMotorVelocityRadPerSec = topMotorEncoder.getVelocity(); + inputs.topLeftMotorCurrentAmps = topLeftMotor.getOutputCurrent(); + inputs.topLeftMotorAppliedVolts = + topLeftMotor.getBusVoltage() * topLeftMotor.getAppliedOutput(); + inputs.topLeftMotorVelocityRadPerSec = topLeftMotorEncoder.getVelocity(); + + inputs.topRightMotorCurrentAmps = topRightMotor.getOutputCurrent(); + inputs.topRightMotorAppliedVolts = + topRightMotor.getBusVoltage() * topRightMotor.getAppliedOutput(); + inputs.topRightMotorVelocityRadPerSec = topRightMotorEncoder.getVelocity(); + + inputs.bottomLeftMotorCurrentAmps = bottomLeftMotor.getOutputCurrent(); + inputs.bottomLeftMotorAppliedVolts = + bottomLeftMotor.getBusVoltage() * bottomLeftMotor.getAppliedOutput(); + inputs.bottomLeftMotorVelocityRadPerSec = bottomLeftMotorEncoder.getVelocity(); + + inputs.bottomRightMotorCurrentAmps = bottomRightMotor.getOutputCurrent(); + inputs.bottomRightMotorAppliedVolts = + bottomRightMotor.getBusVoltage() * bottomRightMotor.getAppliedOutput(); + inputs.bottomRightMotorVelocityRadPerSec = bottomRightMotorEncoder.getVelocity(); inputs.totalAmps = - inputs.middleMotorCurrentAmps + inputs.bottomMotorCurrentAmps + inputs.topMotorCurrentAmps; + inputs.topLeftMotorCurrentAmps + + inputs.topRightMotorCurrentAmps + + inputs.bottomLeftMotorCurrentAmps + + inputs.bottomRightMotorCurrentAmps; inputs.shooterWheelVelocityRadPerSec = - inputs.bottomMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO; - inputs.shooterWheelPosition = bottomMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO; + inputs.bottomLeftMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO; + inputs.shooterWheelPosition = bottomLeftMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO; - inputs.setpointRPM = bottomMotor.getClosedLoopController().getSetpoint(); - inputs.atSetpoint = bottomMotor.getClosedLoopController().isAtSetpoint(); + inputs.setpointRPM = bottomLeftMotor.getClosedLoopController().getSetpoint(); + inputs.atSetpoint = bottomLeftMotor.getClosedLoopController().isAtSetpoint(); } @Override public void setVoltage(double volts) { - bottomMotor.setVoltage(volts); + bottomLeftMotor.setVoltage(volts); + } + + @Override + public void setTopLeftVoltage(double volts) { + topLeftMotor.setVoltage(volts); + } + + @Override + public void setTopRightVoltage(double volts) { + topRightMotor.setVoltage(volts); + } + + @Override + public void setBottomLeftVoltage(double volts) { + bottomLeftMotor.setVoltage(volts); + } + + @Override + public void setBottomRightVoltage(double volts) { + bottomRightMotor.setVoltage(volts); } @Override public void stop() { - bottomMotor.set(0); + topLeftMotor.set(0); + topRightMotor.set(0); + bottomLeftMotor.set(0); + bottomRightMotor.set(0); } private double distanceToRPM(double distanceMeters) {