From 407af199c2fcc39f00d180058fa4bb5040ba980a Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Mon, 16 Dec 2024 16:34:08 -0500 Subject: [PATCH] carson and max c (#77) * carson and max c * Lmao this stuff ez * fix error * add comment push changes * yay * changes = max * Did the ez stuff * ezlmao * ok * ssoafs * fafssaffsfss * safasffsfsafsffsa * saving idk * format and name fix npnp --------- Co-authored-by: mychenezpz <120135435+mychenezpz@users.noreply.github.com> Co-authored-by: Ishan1522 <36376931+NNLUliskey@users.noreply.github.com> --- .../commands/autodrive/AutoAlignWithAmp.java | 26 ++++----- .../commands/intake/IntakeFromGround.java | 4 +- .../commands/shooter/SetFlywheelSpeed.java | 4 +- .../frc/robot/commands/shooter/ShootAmp.java | 10 ++-- .../robot/commands/shooter/ShootSpeaker.java | 26 ++++----- .../commands/shooter/ShootSubwoofer.java | 10 ++-- .../commands/shooter/SpinupFlywheel.java | 4 +- .../robot/subsystems/shooter/Flywheel.java | 14 +++-- ...rConstants.java => FlywheelConstants.java} | 40 +++++++++++++- .../robot/subsystems/shooter/FlywheelIO.java | 13 +++-- .../subsystems/shooter/FlywheelIOSim.java | 10 ++-- .../subsystems/shooter/FlywheelIOTalonFX.java | 53 ++++++++----------- 12 files changed, 124 insertions(+), 90 deletions(-) rename src/main/java/frc/robot/subsystems/shooter/{ShooterConstants.java => FlywheelConstants.java} (72%) diff --git a/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java b/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java index 4fed7570..144c4765 100644 --- a/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java +++ b/src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; import java.util.Optional; @@ -23,24 +23,24 @@ public class AutoAlignWithAmp extends Command { private final ProfiledPIDController turnController = new ProfiledPIDController( - ShooterConstants.AUTO_LINEUP_ROTATION_P, - ShooterConstants.AUTO_LINEUP_ROTATION_I, - ShooterConstants.AUTO_LINEUP_ROTATION_D, - ShooterConstants.AUTO_LINEUP_ROTATION_CONSTRAINTS); + FlywheelConstants.AUTO_LINEUP_ROTATION_P, + FlywheelConstants.AUTO_LINEUP_ROTATION_I, + FlywheelConstants.AUTO_LINEUP_ROTATION_D, + FlywheelConstants.AUTO_LINEUP_ROTATION_CONSTRAINTS); private final ProfiledPIDController xTranslationController = new ProfiledPIDController( - ShooterConstants.AUTO_LINEUP_TRANSLATION_P, - ShooterConstants.AUTO_LINEUP_TRANSLATION_I, - ShooterConstants.AUTO_LINEUP_TRANSLATION_D, - ShooterConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS); + FlywheelConstants.AUTO_LINEUP_TRANSLATION_P, + FlywheelConstants.AUTO_LINEUP_TRANSLATION_I, + FlywheelConstants.AUTO_LINEUP_TRANSLATION_D, + FlywheelConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS); private final ProfiledPIDController yTranslationController = new ProfiledPIDController( - ShooterConstants.AUTO_LINEUP_TRANSLATION_P, - ShooterConstants.AUTO_LINEUP_TRANSLATION_I, - ShooterConstants.AUTO_LINEUP_TRANSLATION_D, - ShooterConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS); + FlywheelConstants.AUTO_LINEUP_TRANSLATION_P, + FlywheelConstants.AUTO_LINEUP_TRANSLATION_I, + FlywheelConstants.AUTO_LINEUP_TRANSLATION_D, + FlywheelConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS); /** Creates a new AutoAlignWithAmp. */ public AutoAlignWithAmp(SwerveDrive swerveDrive, Vision vision) { diff --git a/src/main/java/frc/robot/commands/intake/IntakeFromGround.java b/src/main/java/frc/robot/commands/intake/IntakeFromGround.java index 2de90130..f520bfae 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeFromGround.java +++ b/src/main/java/frc/robot/commands/intake/IntakeFromGround.java @@ -14,7 +14,7 @@ import frc.robot.subsystems.pivot.Pivot; import frc.robot.subsystems.pivot.PivotConstants; import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; public class IntakeFromGround extends Command { private final Intake intake; @@ -59,7 +59,7 @@ public void execute() { public void end(boolean interrupted) { // Ends once the note is in the shooter // Sets roller, intake, and indexer speeds to zero - flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED); intake.setIntakeSpeed(IntakeConstants.INTAKE_NEUTRAL_SPEED); indexer.setIndexerSpeed(IndexerConstants.INDEXER_NEUTRAL_SPEED); // Puts the intake back in the robot diff --git a/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java b/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java index 15c26290..28e3d457 100644 --- a/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java +++ b/src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; import java.util.function.DoubleSupplier; public class SetFlywheelSpeed extends Command { @@ -35,7 +35,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/shooter/ShootAmp.java b/src/main/java/frc/robot/commands/shooter/ShootAmp.java index c3211c1a..95828f38 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootAmp.java +++ b/src/main/java/frc/robot/commands/shooter/ShootAmp.java @@ -8,7 +8,7 @@ import frc.robot.subsystems.elevator.*; import frc.robot.subsystems.pivot.*; import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; import java.util.function.BooleanSupplier; public class ShootAmp extends Command { @@ -36,9 +36,9 @@ public void initialize() {} public void execute() { elevator.setElevatorPosition(ElevatorConstants.SHOOT_AMP_POSITION); pivot.setPivotAngle(PivotConstants.SHOOT_AMP_ANGLE); - flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_AMP_RPM); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_AMP_RPM); if (shoot.getAsBoolean()) { - flywheel.setRollerSpeed(ShooterConstants.ROLLER_SHOOT_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_SHOOT_SPEED); } } @@ -47,8 +47,8 @@ public void execute() { public void end(boolean interrupted) { elevator.setElevatorPosition(ElevatorConstants.INTAKE_POSITION); pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE); - flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED); - flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java b/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java index 012ec173..73d6022c 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java +++ b/src/main/java/frc/robot/commands/shooter/ShootSpeaker.java @@ -15,7 +15,7 @@ import frc.robot.subsystems.pivot.Pivot; import frc.robot.subsystems.pivot.PivotConstants; import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.vision.Vision; import java.util.Optional; @@ -36,10 +36,10 @@ public class ShootSpeaker extends Command { private final ProfiledPIDController turnController = new ProfiledPIDController( - ShooterConstants.AUTO_SHOOT_P, - ShooterConstants.AUTO_SHOOT_I, - ShooterConstants.AUTO_SHOOT_D, - ShooterConstants.AUTO_SHOOT_CONSTRAINTS); + FlywheelConstants.AUTO_SHOOT_P, + FlywheelConstants.AUTO_SHOOT_I, + FlywheelConstants.AUTO_SHOOT_D, + FlywheelConstants.AUTO_SHOOT_CONSTRAINTS); private boolean isRed = false; private double desiredHeading = 0; @@ -108,12 +108,12 @@ public void execute() { !isFieldRelative.getAsBoolean()); // Sets flywheel speed based on distance - // if (distance > ShooterConstants.SHOOTER_FAR_DISTANCE) { - // flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_FAR_RPM); - // } else if (distance > ShooterConstants.SHOOTER_DISTANCE) { - // flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_MEDIUM_RPM); + // if (distance > FlywheelConstants.SHOOTER_FAR_DISTANCE) { + // flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_FAR_RPM); + // } else if (distance > FlywheelConstants.SHOOTER_DISTANCE) { + // flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_MEDIUM_RPM); // } else { - flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_RPM); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_RPM); // } // Sets Pivot and Elevator @@ -122,7 +122,7 @@ public void execute() { if (isReadyToShoot()) { // Pushes note to flywheels once robot is ready - flywheel.setRollerSpeed(ShooterConstants.ROLLER_SHOOT_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_SHOOT_SPEED); } else { // Don't shoot } @@ -131,8 +131,8 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED); - flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED); pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE); elevator.setElevatorPosition(ElevatorConstants.INTAKE_POSITION); } diff --git a/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java b/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java index a148c5ec..8a46cbc8 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java +++ b/src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java @@ -8,7 +8,7 @@ import frc.robot.subsystems.pivot.Pivot; import frc.robot.subsystems.pivot.PivotConstants; import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; public class ShootSubwoofer extends Command { private final Pivot pivot; @@ -29,19 +29,19 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_RPM); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_RPM); pivot.setPivotAngle(PivotConstants.SUBWOOFER_ANGLE); if (pivot.isPivotWithinAcceptableError()) { - flywheel.setRollerSpeed(ShooterConstants.ROLLER_SHOOT_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_SHOOT_SPEED); } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED); + flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED); pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE); - flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED); + flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/shooter/SpinupFlywheel.java b/src/main/java/frc/robot/commands/shooter/SpinupFlywheel.java index 5e9e5e6b..537e424b 100644 --- a/src/main/java/frc/robot/commands/shooter/SpinupFlywheel.java +++ b/src/main/java/frc/robot/commands/shooter/SpinupFlywheel.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.Flywheel; -import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.shooter.FlywheelConstants; public class SpinupFlywheel extends Command { @@ -32,7 +32,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - flywheelSubsystem.setFlywheelVelocity(ShooterConstants.FLYWHEEL_SPINUP_SPEED); + flywheelSubsystem.setFlywheelVelocity(FlywheelConstants.FLYWHEEL_SPINUP_SPEED); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java index ba81cd8f..8d068b9c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java @@ -7,8 +7,7 @@ public class Flywheel extends SubsystemBase { private final FlywheelIO io; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - // private final SimpleMotorFeedforward ffModel; - + /** Constructor that uses IO. IO methods can be used in subsystem. */ public Flywheel(FlywheelIO io) { this.io = io; } @@ -16,10 +15,10 @@ public Flywheel(FlywheelIO io) { /** * Sets the flywheel voltage * - * @param desiredVoltage + * @param desiredVoltage Desired Voltage in double */ public void setFlywheelVoltage(double desiredVoltage) { - io.setVoltage(desiredVoltage); // /io calls the functions + io.setVoltage(desiredVoltage); // /IO calls the functions Logger.recordOutput("Flywheel/voltage", desiredVoltage); } @@ -33,6 +32,11 @@ public void setFlywheelVelocity(double desiredRPM) { Logger.recordOutput("Flywheel/RPM", desiredRPM); } + /** + * Returns whether or not a note is detected + * + * @return False if not detected, True if detected + */ public boolean hasNote() { return inputs.isNoteDetected; } @@ -51,7 +55,7 @@ public void setRollerSpeed(double speed) { public void periodic() { // This method will be called once per scheduler run io.updateInputs(inputs); - Logger.processInputs("FlywheelSubsystem", inputs); + Logger.processInputs("Flywheel", inputs); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelConstants.java similarity index 72% rename from src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java rename to src/main/java/frc/robot/subsystems/shooter/FlywheelConstants.java index 2b25faf7..26248513 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelConstants.java @@ -4,17 +4,33 @@ import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants; -public class ShooterConstants { +/** All of the constants called in the shooter subsystems. */ +public class FlywheelConstants { + /** ID of the leader flywheel */ public static final int LEADER_FLYWHEEL_ID = 4; + + /** ID of the follower flywheel */ public static final int FOLLOWER_FLYWHEEL_ID = 12; + + /** ID of the roller motor */ public static final int ROLLER_MOTOR_ID = 2; + /** Sets the limit for the shooter supply voltage */ public static final double SHOOTER_SUPPLY_LIMIT = 60; + + /** Sets the current limit for the motor stator */ public static final double SHOOTER_STATOR_LIMIT = 60; + + /** Enables the limit for stator */ public static final boolean SHOOTER_STATOR_ENABLE = true; + + /** Enables the limit for current to motor */ public static final boolean SHOOTER_SUPPLY_ENABLE = true; + /** Sets the speed of the roller motor for neutral mode */ public static final double ROLLER_NEUTRAL_SPEED = 0; + + /** Sets the speed of the shooter motor for neutral mode */ public static final double SHOOTER_NEUTRAL_SPEED = 0; public static final double FLYWHEEL_SPINUP_SPEED = 4000; @@ -24,11 +40,22 @@ public class ShooterConstants { public static final int SHOOTER_ACCEPTABLE_RPM_ERROR = 25; + /** Forced Labor has determined that this is the best P value */ public static final double SHOOT_P = 0.522; + + /** Forced Labor has determined that this is the best I value */ public static final double SHOOT_I = 0.00; + + /** Forced Labor has determined that this is the best D value */ public static final double SHOOT_D = 0.001; + + /** Forced Labor has determined that this is the best S value */ public static final double SHOOT_S = 0.319692618511411; + + /** Forced Labor has determined that this is the best V value */ public static final double SHOOT_V = 0.125930273774783; + + /** Forced Labor has determined that this is the best A value */ public static final double SHOOT_A = 0.004358865417933; public static final double ROLLER_SHOOT_SPEED = 1; @@ -78,9 +105,20 @@ public class ShooterConstants { public static final double FLYWHEEL_SUPPLY_LIMIT = 60.0; public static final boolean FLYWHEEL_SUPPLY_ENABLE = true; + + /** + * The stator limit limits the current sent to the stator in the motor + * https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html + */ public static final double FLYWHEEL_STATOR_LIMIT = 60.0; + + /** + * Enables the stator limit which limits current to the stator in the motor + * https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html + */ public static final boolean FLYWHEEL_STATOR_ENABLE = true; public static final double LOW_VOLTAGE_BOUNDARY = 12.0; + public static final double RPM_RPS_CONVERSION_FACTOR = 60.0; public static final double HIGH_VOLTAGE_BOUNDARY = -12.0; } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index a0ac97f5..06f031eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -4,8 +4,9 @@ public interface FlywheelIO { @AutoLog - public static class FlywheelIOInputs { // variables that we want to log - public double positionRotations = 0.0; // positions in radians | convert to rpms + /** Creates new variables necessary for logging */ + public static class FlywheelIOInputs { // Variables that we want to log + public double positionRotations = 0.0; // Positions in radians | convert to rpms public double velocityRPM = 0.0; public double appliedVolts = 0.0; public double[] currentAmps = new double[] {}; @@ -21,13 +22,17 @@ public default void setVoltage(double volts) {} /** * Run closed loop at the specified velocity. * - * @param velocityRPM Recieve desired input in rounds per minute, and the method will convert to - * RPS to match requirements for VelocityVoltage + * @param velocityRPM Recieve desired input in rotations per minute */ public default void setVelocity(double velocityRPM) {} /** Stop in open loop. */ public default void stop() {} + /** + * Sets speed of the roller + * + * @param speed Range is -1 to 1 + */ default void setRollerSpeed(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index e4bc56be..107ad89c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -11,13 +11,13 @@ public class FlywheelIOSim implements FlywheelIO { // FlywheelIOSim makes Advantage kit log simulated movements using // physics private FlywheelSim flywheelSim = new FlywheelSim(null, DCMotor.getKrakenX60(2), 0); - private DIOSim noteSensorSim = new DIOSim(new DigitalInput(ShooterConstants.NOTE_SENSOR_ID)); + private DIOSim noteSensorSim = new DIOSim(new DigitalInput(FlywheelConstants.NOTE_SENSOR_ID)); private PIDController pid = new PIDController( - ShooterConstants.FLYWHEEL_P, ShooterConstants.FLYWHEEL_I, ShooterConstants.FLYWHEEL_D); + FlywheelConstants.FLYWHEEL_P, FlywheelConstants.FLYWHEEL_I, FlywheelConstants.FLYWHEEL_D); private SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward( - ShooterConstants.FLYWHEEL_S, ShooterConstants.FLYWHEEL_V, ShooterConstants.FLYWHEEL_A); + FlywheelConstants.FLYWHEEL_S, FlywheelConstants.FLYWHEEL_V, FlywheelConstants.FLYWHEEL_A); private double appliedVolts = 0.0; @@ -38,10 +38,6 @@ public void setVoltage(double volts) { flywheelSim.setInputVoltage(volts); } - /** - * @param velocityRPM User inputs the desired velocity in RPM, gets converted in method for PID to - * set value in RPS - */ @Override public void setVelocity(double velocityRPM) { // ffvolts is feedorward double velocityRPS = velocityRPM / 60; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java index 030a5ca0..3618d890 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java @@ -18,16 +18,15 @@ public class FlywheelIOTalonFX implements FlywheelIO { // FlywheelIOTalonFX makes Advantagekit log physical hardware movement private final TalonFX leftFlywheelMotor = - new TalonFX(ShooterConstants.LEFT_FLYWHEEL_MOTOR_ID); // Leader=left motor + new TalonFX(FlywheelConstants.LEFT_FLYWHEEL_MOTOR_ID); // Leader=left motor private final TalonFX rightFlywheelMotor = - new TalonFX(ShooterConstants.RIGHT_FLYWHEEL_MOTOR_ID); // follower = right motor + new TalonFX(FlywheelConstants.RIGHT_FLYWHEEL_MOTOR_ID); // follower = right motor private final DigitalInput noteSensor = - new DigitalInput(ShooterConstants.NOTE_SENSOR_ID); // Note sensor - private final TalonFX rollerMotor = new TalonFX(ShooterConstants.ROLLER_MOTOR_ID); - + new DigitalInput(FlywheelConstants.NOTE_SENSOR_ID); // Note sensor + private final TalonFX rollerMotor = new TalonFX(FlywheelConstants.ROLLER_MOTOR_ID); + // gives values for each thing that is set. private final VelocityVoltage velocityRequest; private final VoltageOut voltageRequest; - // gives values for each thing that is set. private final StatusSignal leaderPosition = leftFlywheelMotor.getPosition(); private final StatusSignal leaderVelocity = leftFlywheelMotor.getVelocity(); private final StatusSignal leaderAppliedVolts = leftFlywheelMotor.getMotorVoltage(); @@ -41,23 +40,25 @@ public class FlywheelIOTalonFX public FlywheelIOTalonFX() { // Object to set different flywheel configs TalonFXConfiguration flywheelConfig = new TalonFXConfiguration(); flywheelConfig.CurrentLimits.SupplyCurrentLimit = - ShooterConstants - .FLYWHEEL_SUPPLY_LIMIT; // Talonfx configuration software limits found in CONSTANTS file - flywheelConfig.CurrentLimits.StatorCurrentLimit = ShooterConstants.FLYWHEEL_STATOR_LIMIT; - flywheelConfig.CurrentLimits.SupplyCurrentLimitEnable = ShooterConstants.FLYWHEEL_SUPPLY_ENABLE; - flywheelConfig.CurrentLimits.StatorCurrentLimitEnable = ShooterConstants.FLYWHEEL_STATOR_ENABLE; + FlywheelConstants.FLYWHEEL_SUPPLY_LIMIT; // Talonfx configuration software limits found in + // FlywheelConstants file + flywheelConfig.CurrentLimits.StatorCurrentLimit = FlywheelConstants.FLYWHEEL_STATOR_LIMIT; + flywheelConfig.CurrentLimits.SupplyCurrentLimitEnable = + FlywheelConstants.FLYWHEEL_SUPPLY_ENABLE; + flywheelConfig.CurrentLimits.StatorCurrentLimitEnable = + FlywheelConstants.FLYWHEEL_STATOR_ENABLE; flywheelConfig.MotorOutput.NeutralMode = NeutralModeValue .Coast; // This is used to ensure maximum power efficiency, to ensure flywheels keep // spinning after power off. flywheelConfig.Slot0.kP = - ShooterConstants.FLYWHEEL_P; // everything tuned in Flywheel Constants file - flywheelConfig.Slot0.kI = ShooterConstants.FLYWHEEL_I; - flywheelConfig.Slot0.kP = ShooterConstants.FLYWHEEL_D; - flywheelConfig.Slot0.kS = ShooterConstants.FLYWHEEL_S; - flywheelConfig.Slot0.kV = ShooterConstants.FLYWHEEL_V; - flywheelConfig.Slot0.kA = ShooterConstants.FLYWHEEL_A; + FlywheelConstants.FLYWHEEL_P; // everything tuned in Flywheel Constants file + flywheelConfig.Slot0.kI = FlywheelConstants.FLYWHEEL_I; + flywheelConfig.Slot0.kP = FlywheelConstants.FLYWHEEL_D; + flywheelConfig.Slot0.kS = FlywheelConstants.FLYWHEEL_S; + flywheelConfig.Slot0.kV = FlywheelConstants.FLYWHEEL_V; + flywheelConfig.Slot0.kA = FlywheelConstants.FLYWHEEL_A; leftFlywheelMotor .getConfigurator() @@ -90,18 +91,15 @@ public FlywheelIOTalonFX() { // Object to set different flywheel configs voltageRequest = new VoltageOut(0.0); } - /** - * @param inputs has all the inputs, updates and logs them - */ @Override public void updateInputs( FlywheelIOInputs inputs) { // gets current values for motors and puts them into a log BaseStatusSignal.refreshAll( leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.positionRotations = - leaderPosition.getValueAsDouble() - / ShooterConstants.GEAR_RATIO; // converted to radians to gear ratio math - inputs.velocityRPM = (leaderVelocity.getValueAsDouble() * 60.0) / ShooterConstants.GEAR_RATIO; + inputs.positionRotations = leaderPosition.getValueAsDouble(); // gear ratio is one + inputs.velocityRPM = + (leaderVelocity.getValueAsDouble() * FlywheelConstants.RPM_RPS_CONVERSION_FACTOR) + / FlywheelConstants.GEAR_RATIO; inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); inputs.currentAmps = new double[] { @@ -115,10 +113,6 @@ public void setVoltage(double volts) { // some method to set voltage for motor leftFlywheelMotor.setControl(voltageRequest.withOutput(volts)); } - /** - * @param velocityRPM Recieve desired input in rounds per minute, and the method will convert to - * RPS to match requirements for VelocityVoltage * - */ @Override public void setVelocity(double velocityRPM) { leftFlywheelMotor.setControl(velocityRequest.withVelocity(velocityRPM / 60.0)); @@ -133,9 +127,6 @@ public void setRollerSpeed(double speed) { rollerMotor.set(speed); } - /** - * @return Whether the sensor can detect a note in there - */ public boolean hasNote() { return !noteSensor.get(); }