From 1ed3e34386cbffee6840723732bf778985b5291e Mon Sep 17 00:00:00 2001 From: ahoysal Date: Sat, 9 Mar 2024 14:10:54 -0800 Subject: [PATCH] initial thing with enum --- src/main/java/frc/robot/Constants.java | 29 +++++-- .../frc/robot/subsystems/PeterSubsystem.java | 75 +++++++------------ 2 files changed, 47 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65307b38..4492c5e8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,28 +49,41 @@ public static class OperatorConstants { public static final int BIG_BUTTON_PORT = 14; } - public static final class Peter { + public static final class Pooer { public static final int INTAKE_MOTOR_PORT = 33; - // public static final double SHOOTER_SPEED = 3; // Shooter gear ratio: 15:12 public static final int NOTE_DETECTOR_PORT = 1; public static final int PRE_SHOOTER_PORT = 32; - public static final int SHOOTER_PORT_RIGHT = 30; - public static final int SHOOTER_PORT_LEFT = 31; public static final double INTAKE_WHEEL_SPEED_RPS = 200; // Intake gear ratio: 2:1 public static final double ROTATIONS_TO_SHOOTER = 300; // Preshooter gear ratio: 4:1 - public static final double SHOOT_WHEEL_SPEED_RPS = - 3500.0 / 60.0; // TODO: Add right and left motor RPS, and change it to not max public static final double PRESHOOTER_WHEEL_VOLTAGE = 9; public static final String CANBUS_NAME = "rio"; public static final double INTAKE_GEAR_RATIO = 2; public static final double PRESHOOTER_GEAR_RATIO = 4; - public static final double SHOOTER_WHEELS_GEAR_RATIOS = 24d / 18d; - public static final double SHOOTER_STATOR_CURRENT_LIMIT_AMPS = 40.0; public static final double PRESHOOTER_STATOR_CURRENT_LIMIT_AMPS = 25.0; public static final double INTAKE_STATOR_CURRENT_LIMIT_AMPS = 50.0; + + public static final ShooterType SHOOTER = ShooterType.PETER; + + public static enum ShooterType { + PETER(30, 31, 4500.0 / 60.0, 12d / 15d, 40.0), + PIPER(30, 31, 3500.0 / 60.0, 24d / 18d, 40.0); + public final int PORT_1, PORT_2; + public final double SPEED_RPS; + public final double GEAR_RATIO; + public final double STATOR_CURRENT_LIMIT_AMPS; + + ShooterType( + int port1, int port2, double speedRPS, double gearRatio, double statorCurrentLimitAmp) { + PORT_1 = port1; + PORT_2 = port2; + SPEED_RPS = speedRPS; + GEAR_RATIO = gearRatio; + STATOR_CURRENT_LIMIT_AMPS = statorCurrentLimitAmp; + } + } } public static final class Arm { diff --git a/src/main/java/frc/robot/subsystems/PeterSubsystem.java b/src/main/java/frc/robot/subsystems/PeterSubsystem.java index e90c52fc..88ff154b 100644 --- a/src/main/java/frc/robot/subsystems/PeterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PeterSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; @@ -21,37 +20,30 @@ public class PeterSubsystem extends SubsystemBase { private DigitalInput noteSensor; private TalonFX shooterMotorUp, shooterMotorDown; private TalonFX preShooterMotor, intakeMotor; - private StatusSignal preShooterPosition; private MotionMagicConfigs mmcPreShooter; public PeterSubsystem() { // Initalize shooter // Follower f = new Follower(Constants.Intake.SHOOTER_PORT_LEFT, false ); - shooterMotorDown = new TalonFX(Constants.Peter.SHOOTER_PORT_LEFT, Constants.Peter.CANBUS_NAME); - shooterMotorUp = new TalonFX(Constants.Peter.SHOOTER_PORT_RIGHT, Constants.Peter.CANBUS_NAME); + shooterMotorDown = new TalonFX(Constants.Pooer.SHOOTER.PORT_1, Constants.Pooer.CANBUS_NAME); + shooterMotorUp = new TalonFX(Constants.Pooer.SHOOTER.PORT_2, Constants.Pooer.CANBUS_NAME); shooterMotorUp.setInverted(true); // shooterMotorRight.setControl(f); Slot0Configs s0c = new Slot0Configs().withKP(0.001).withKI(0).withKD(0).withKG(0).withKV(0.2).withKA(0); + CurrentLimitsConfigs clc = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(Constants.Pooer.SHOOTER.STATOR_CURRENT_LIMIT_AMPS); shooterMotorUp.getConfigurator().apply(s0c); shooterMotorDown.getConfigurator().apply(s0c); - shooterMotorDown - .getConfigurator() - .apply( - new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(Constants.Peter.SHOOTER_STATOR_CURRENT_LIMIT_AMPS)); - shooterMotorUp - .getConfigurator() - .apply( - new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(Constants.Peter.SHOOTER_STATOR_CURRENT_LIMIT_AMPS)); + shooterMotorUp.getConfigurator().apply(clc); + shooterMotorDown.getConfigurator().apply(clc); // Preshooter - preShooterMotor = new TalonFX(Constants.Peter.PRE_SHOOTER_PORT, Constants.Peter.CANBUS_NAME); + preShooterMotor = new TalonFX(Constants.Pooer.PRE_SHOOTER_PORT, Constants.Pooer.CANBUS_NAME); mmcPreShooter = new MotionMagicConfigs(); mmcPreShooter.MotionMagicCruiseVelocity = 80; mmcPreShooter.MotionMagicAcceleration = 160; @@ -65,23 +57,21 @@ public PeterSubsystem() { .apply( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(Constants.Peter.PRESHOOTER_STATOR_CURRENT_LIMIT_AMPS)); + .withStatorCurrentLimit(Constants.Pooer.PRESHOOTER_STATOR_CURRENT_LIMIT_AMPS)); // Intake Slot0Configs intakePid = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKG(0).withKV(0).withKA(0); - intakeMotor = new TalonFX(Constants.Peter.INTAKE_MOTOR_PORT, Constants.Peter.CANBUS_NAME); + intakeMotor = new TalonFX(Constants.Pooer.INTAKE_MOTOR_PORT, Constants.Pooer.CANBUS_NAME); intakeMotor.getConfigurator().apply(intakePid); intakeMotor .getConfigurator() .apply( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(Constants.Peter.INTAKE_STATOR_CURRENT_LIMIT_AMPS)); + .withStatorCurrentLimit(Constants.Pooer.INTAKE_STATOR_CURRENT_LIMIT_AMPS)); intakeMotor.setInverted(true); - noteSensor = new DigitalInput(Constants.Peter.NOTE_DETECTOR_PORT); - - preShooterPosition = preShooterMotor.getPosition(); + noteSensor = new DigitalInput(Constants.Pooer.NOTE_DETECTOR_PORT); } public static PeterSubsystem getInstance() { @@ -93,12 +83,12 @@ public static PeterSubsystem getInstance() { // INTAKE FUNCTIONS: public void spinUpIntake() { - runIntakeAtRPS(Constants.Peter.INTAKE_WHEEL_SPEED_RPS); + runIntakeAtRPS(Constants.Pooer.INTAKE_WHEEL_SPEED_RPS); } private void runIntakeAtRPS(double speed) { VelocityVoltage m_velocityControl = - new VelocityVoltage(speed * Constants.Peter.INTAKE_GEAR_RATIO); + new VelocityVoltage(speed * Constants.Pooer.INTAKE_GEAR_RATIO); m_velocityControl.withFeedForward(0.1); intakeMotor.setControl(m_velocityControl); } @@ -110,7 +100,7 @@ public void stopIntake() { // SHOOTER FUNCTIONS: private void runRightShooterAtRPS(double speed) { VelocityVoltage m_velocityControl = - new VelocityVoltage(speed * Constants.Peter.SHOOTER_WHEELS_GEAR_RATIOS); + new VelocityVoltage(speed * Constants.Pooer.SHOOTER.GEAR_RATIO); m_velocityControl.withFeedForward(0.1); shooterMotorUp.setControl(m_velocityControl); shooterMotorUp.getVelocity(); @@ -118,7 +108,7 @@ private void runRightShooterAtRPS(double speed) { private void runLeftShooterAtRPS(double speed) { VelocityVoltage m_velocityControl = - new VelocityVoltage(speed * Constants.Peter.SHOOTER_WHEELS_GEAR_RATIOS); + new VelocityVoltage(speed * Constants.Pooer.SHOOTER.GEAR_RATIO); m_velocityControl.withFeedForward(0.1); shooterMotorDown.setControl(m_velocityControl); } @@ -134,11 +124,11 @@ public void stopShooter(boolean forceStop) { } public void spinRightShooter() { - runRightShooterAtRPS(Constants.Peter.SHOOT_WHEEL_SPEED_RPS); + runRightShooterAtRPS(Constants.Pooer.SHOOTER.SPEED_RPS); } public void spinLeftShooter() { - runLeftShooterAtRPS(Constants.Peter.SHOOT_WHEEL_SPEED_RPS); + runLeftShooterAtRPS(Constants.Pooer.SHOOTER.SPEED_RPS); } public void stopRightShooter() { @@ -154,13 +144,13 @@ public void resetPreshooterPosition() { } public void reversePreshooterRotations(double count) { - preShooterMotor.setControl(new PositionVoltage(-count * Constants.Peter.PRESHOOTER_GEAR_RATIO)); + preShooterMotor.setControl(new PositionVoltage(-count * Constants.Pooer.PRESHOOTER_GEAR_RATIO)); } public boolean isBackedUp(double count) { return Math.abs( preShooterMotor.getPosition().getValueAsDouble() - - (-count * Constants.Peter.PRESHOOTER_GEAR_RATIO)) + - (-count * Constants.Pooer.PRESHOOTER_GEAR_RATIO)) < 0.1; } @@ -172,18 +162,10 @@ public void reverseMechanism() { } public boolean isShooterReady() { - if (Math.abs(shooterMotorDown.getVelocity().getValue() - Constants.Peter.SHOOT_WHEEL_SPEED_RPS) - < 0.001) { - return true; - } - if (Math.abs( + return Math.abs( shooterMotorDown.getVelocity().getValueAsDouble() - - (Constants.Peter.SHOOT_WHEEL_SPEED_RPS - * Constants.Peter.SHOOTER_WHEELS_GEAR_RATIOS)) - < 10) { - return true; - } - return false; + - (Constants.Pooer.SHOOTER.SPEED_RPS * Constants.Pooer.SHOOTER.GEAR_RATIO)) + < 10; } /* private void runShooterAtRPS(double speed) { @@ -204,11 +186,11 @@ public boolean notePresent() { // PRE-SHOOTER FUNCTIONS: public void spinUpPreShooter() { - runPreShooterAtRPS(Constants.Peter.ROTATIONS_TO_SHOOTER); + runPreShooterAtRPS(Constants.Pooer.ROTATIONS_TO_SHOOTER); } private void runPreShooterAtRPS(double speed) { - VoltageOut velocityControl = new VoltageOut(Constants.Peter.PRESHOOTER_WHEEL_VOLTAGE); + VoltageOut velocityControl = new VoltageOut(Constants.Pooer.PRESHOOTER_WHEEL_VOLTAGE); preShooterMotor.setControl(velocityControl); // VelocityVoltage m_velocityControl = @@ -278,9 +260,4 @@ public void periodicSignalLogger() { SignalLogger.writeDouble( "Shooter up current", shooterMotorUp.getStatorCurrent().getValueAsDouble()); } - - public void runShooter(int i) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'runShooter'"); - } }