From ddd4f11efba2fd36bb2c4313599ea77805aff879 Mon Sep 17 00:00:00 2001 From: moo Date: Sun, 12 Apr 2026 12:40:34 -0700 Subject: [PATCH] shooter rewrite/cleanup, advantagekit should work on it now --- .../java/frc/robot/commands/LogCommand.java | 4 + .../frc/robot/subsystems/Intake/IntakeIO.java | 16 +- .../subsystems/Intake/IntakeIOTalonFX.java | 188 ++++++++++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 111 ++--------- .../robot/subsystems/shooter/ShooterIO.java | 22 +- .../subsystems/shooter/ShooterIOTalonFX.java | 80 ++++++++ 6 files changed, 309 insertions(+), 112 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java diff --git a/src/main/java/frc/robot/commands/LogCommand.java b/src/main/java/frc/robot/commands/LogCommand.java index 06b346d..d3f03ed 100644 --- a/src/main/java/frc/robot/commands/LogCommand.java +++ b/src/main/java/frc/robot/commands/LogCommand.java @@ -2,6 +2,7 @@ package frc.robot.commands; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.util.Elastic; @@ -30,6 +31,9 @@ public class LogCommand extends Command { } hubActive = current; + + // keep this + SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java index 7380a7b..037805e 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -25,28 +25,28 @@ public interface IntakeIO { * * @return inches */ - double getPosition(); + public double getPosition(); /** * Set the intake extender position * * @param setpoint in inches */ - void setPosition(double setpoint); + public void setPosition(double setpoint); - void setLeftMotor(double speed); + public void setLeftMotor(double speed); - void setRightMotor(double speed); + public void setRightMotor(double speed); - void setRollerMotor(double speed); + public void setRollerMotor(double speed); - void setLimits(CurrentLimitsConfigs limits); + public void setLimits(CurrentLimitsConfigs limits); - void setRawPosition(double pos); + public void setRawPosition(double pos); /** * Reclaim all the resources (e.g., motors and other devices). * This step is necessary for multiple unit tests to work. */ - void close(); + public void close(); } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java new file mode 100644 index 0000000..f35be40 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOTalonFX.java @@ -0,0 +1,188 @@ +package frc.robot.subsystems.Intake; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color8Bit; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; +import frc.robot.constants.IntakeConstants; + +public class IntakeIOTalonFX implements IntakeIO { + + // create the motors + /** Motor to move the roller */ + private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB); + /** Right motor (master) */ + private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB); + /** Left motor (slave) */ + private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB); + + private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); + + /** + * Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox) + */ + private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1); + /** + * Motor characteristics for the extending pair of Kraken X44 motors (aka + * gearbox) + */ + private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2); + + public IntakeIOTalonFX() { + + // get the maximum free speed + double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec) / IntakeConstants.GEAR_RATIO; + + // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio + // safety margin, limits velocity to .75 free speed + double maxVelocity = 0.75 * maxFreeSpeed; + double maxAcceleration = maxVelocity / 0.25; + + // Configure the motors + // Build the configuration for the roller + TalonFXConfiguration rollerConfig = new TalonFXConfiguration(); + + // config Slot 0 PID params + var slot0Configs = rollerConfig.Slot0; + slot0Configs.kP = 5.0; + slot0Configs.kI = 0.0; + slot0Configs.kD = 0.0; + slot0Configs.kV = 0.0; + slot0Configs.kA = 0.0; + + // set the brake mode + rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake); + + // apply the configuration to the right motor (master) + rollerMotor.getConfigurator().apply(rollerConfig); + + // Build the configuration for the left and right Motor + TalonFXConfiguration config = new TalonFXConfiguration(); + + // config the current limits (low value for testing) + config.CurrentLimits + .withStatorCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS) + .withSupplyCurrentLimitEnable(true); + + // config Slot 0 PID params + var rollerSlot0Configs = config.Slot0; + rollerSlot0Configs.kP = 5.0; + rollerSlot0Configs.kI = 0.0; + rollerSlot0Configs.kD = 0.0; + rollerSlot0Configs.kV = 0.0; + rollerSlot0Configs.kA = 0.0; + + // configure MotionMagic + MotionMagicConfigs motionMagicConfigs = config.MotionMagic; + + motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity + / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2; + motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration + / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2; + + rightMotor.getConfigurator().apply(config); + leftMotor.getConfigurator().apply(config); + + leftMotor.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast)); + + rightMotor.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)); + + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = IntakeConstants.NORMAL_CURRENT_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + leftMotor.getConfigurator().apply(limitConfig); + rightMotor.getConfigurator().apply(limitConfig); + + leftMotor.setPosition(0.0); + rightMotor.setPosition(0.0); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.leftPosition = Intake.rotationsToInches(leftMotor.getPosition().getValueAsDouble()); + inputs.rightPosition = Intake.rotationsToInches(rightMotor.getPosition().getValueAsDouble()); + inputs.leftCurrent = leftMotor.getStatorCurrent().getValueAsDouble(); + inputs.rightCurrent = rightMotor.getStatorCurrent().getValueAsDouble(); + inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble(); + inputs.rollerCurrent = rollerMotor.getStatorCurrent().getValueAsDouble(); + inputs.rightVoltage = rightMotor.getMotorVoltage().getValueAsDouble(); + inputs.leftVoltage = leftMotor.getMotorVoltage().getValueAsDouble(); + inputs.rollerSetSpeed = rollerMotor.get(); + } + + /** + * Set the intake extender position + * + * @param setpoint in inches + */ + @Override + public void setPosition(double setpoint) { + double motorRotations = -Intake.inchesToRotations(setpoint); + rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true)); + leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true)); + } + + /** + * Get the intake extender position + * + * @return inches + */ + @Override + public double getPosition() { + return Intake.rotationsToInches(leftMotor.getPosition().getValueAsDouble()); + } + + @Override + public void setLeftMotor(double speed) { + leftMotor.set(speed); + } + + @Override + public void setRightMotor(double speed) { + rightMotor.set(speed); + } + + @Override + public void setRollerMotor(double speed) { + rollerMotor.set(speed); + } + + @Override + public void setLimits(CurrentLimitsConfigs limits) { + leftMotor.getConfigurator().apply(limits); + rightMotor.getConfigurator().apply(limits); + } + + @Override + public void setRawPosition(double pos) { + leftMotor.setPosition(pos); + rightMotor.setPosition(pos); + } + + + @Override + public void close() { + leftMotor.close(); + rightMotor.close(); + rollerMotor.close(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ab2b34d..917e118 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,79 +4,31 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; -import frc.robot.util.HubActive; -public class Shooter extends SubsystemBase implements ShooterIO { - - private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB); - private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB); +public class Shooter extends SubsystemBase { + private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - //TODO Add current limits + LoggedNetworkNumber powerModifier = new LoggedNetworkNumber("OPERATOR: Shooter Power Modifier", 1.05); + ShooterIO io; + // Goal Velocity / Double theCircumfrence private double shooterTargetSpeed = 0; - // Velocity in rotations per second - VelocityVoltage voltageRequest = new VelocityVoltage(0); - - private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - - - LoggedNetworkNumber powerModifier = new LoggedNetworkNumber("OPERATOR: Shooter Power Modifier", 1.05); - - public Shooter() { - updateInputs(); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.5; // 0.5 stable - config.Slot0.kI = 0; - config.Slot0.kD = 0.0; - config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps - - config.CurrentLimits - .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT) - .withSupplyCurrentLimitEnable(true); - - shooterMotorLeft.getConfigurator().apply(config); - shooterMotorRight.getConfigurator().apply(config); - - shooterMotorLeft.getConfigurator().apply( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast) - ); - - shooterMotorRight.getConfigurator().apply( - new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) - ); - - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT; - limitConfig.StatorCurrentLimitEnable = true; - shooterMotorLeft.getConfigurator().apply(limitConfig); - shooterMotorRight.getConfigurator().apply(limitConfig); - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0))); - } + public Shooter(ShooterIO io) { + this.io = io; + io.updateInputs(inputs); } @Override public void periodic(){ - updateInputs(); + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); // shooterTargetSpeed = SmartDashboard.getNumber("Shooter Setpoint", shooterTargetSpeed); // SmartDashboard.putNumber("Shooter Setpoint", shooterTargetSpeed); @@ -85,30 +37,14 @@ public class Shooter extends SubsystemBase implements ShooterIO { // Convert to RPS double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)) * powerModifier.get(); - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Target Velocity RPS", targetVelocityRPS); - SmartDashboard.putNumber("Shooter Motor RPS", shooterMotorLeft.getVelocity().getValueAsDouble()); - } // Sets the motor control to target velocity - shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true)); - shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true)); + io.setTargetVelocityRps(targetVelocityRPS); if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER); + Logger.recordOutput("Shooter/realVelocity", inputs.shooterSpeedLeft); Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); } - - double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity); - SmartDashboard.putBoolean("Shooter At Speed", atTargetSpeed()); - SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0); - } - - // keep this - SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); } /** @@ -124,25 +60,6 @@ public class Shooter extends SubsystemBase implements ShooterIO { return inputs.shooterSpeedLeft; } - public void setNewCurrentLimit(double newCurrentLimit) { - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = newCurrentLimit; - limitConfig.StatorCurrentLimitEnable = true; - shooterMotorLeft.getConfigurator().apply(limitConfig); - shooterMotorRight.getConfigurator().apply(limitConfig); - } - - @Override - public void updateInputs(){ - inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; - inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())* ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; - inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); - inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble(); - - - Logger.processInputs("Shooter", inputs); - } - public void bumpUpShooterModifier() { powerModifier.set(powerModifier.get() + 0.025); } @@ -158,6 +75,10 @@ public class Shooter extends SubsystemBase implements ShooterIO { return Math.abs(getShooterVelocity() - shooterTargetSpeed) < 1.0; } + public void setNewCurrentLimits(double limit) { + io.setNewCurrentLimit(limit); + } + /** * @return Gets the target velocity in m/s */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index b698a1f..ffed074 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -3,13 +3,17 @@ package frc.robot.subsystems.shooter; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { - @AutoLog - public static class ShooterIOInputs { - public double shooterSpeedLeft = 0.0; - public double shooterSpeedRight = 0.0; - public double shooterCurrentLeft = 0.0; - public double shooterCurrentRight = 0.0; - } - - public void updateInputs(); + @AutoLog + public static class ShooterIOInputs { + public double shooterSpeedLeft = 0.0; + public double shooterSpeedRight = 0.0; + public double shooterCurrentLeft = 0.0; + public double shooterCurrentRight = 0.0; + } + + public void updateInputs(ShooterIOInputs inputs); + + public void setNewCurrentLimit(double newCurrentLimit); + + public void setTargetVelocityRps(double target); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java new file mode 100644 index 0000000..1277caf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.constants.IdConstants; + +public class ShooterIOTalonFX implements ShooterIO { + + private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB); + private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB); + + // TODO Add current limits + + // Velocity in rotations per second + VelocityVoltage voltageRequest = new VelocityVoltage(0); + + public ShooterIOTalonFX() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.5; // 0.5 stable + config.Slot0.kI = 0; + config.Slot0.kD = 0.0; + config.Slot0.kV = 0.125; // Maximum rps = 100 --> 12V/100rps + + config.CurrentLimits + .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT) + .withSupplyCurrentLimitEnable(true); + + shooterMotorLeft.getConfigurator().apply(config); + shooterMotorRight.getConfigurator().apply(config); + + shooterMotorLeft.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast)); + + shooterMotorRight.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)); + + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + shooterMotorLeft.getConfigurator().apply(limitConfig); + shooterMotorRight.getConfigurator().apply(limitConfig); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) + * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2; + inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble()) + * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2; + inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble(); + inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble(); + + } + + @Override + public void setNewCurrentLimit(double newCurrentLimit) { + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = newCurrentLimit; + limitConfig.StatorCurrentLimitEnable = true; + shooterMotorLeft.getConfigurator().apply(limitConfig); + shooterMotorRight.getConfigurator().apply(limitConfig); + } + + @Override + public void setTargetVelocityRps(double target) { + shooterMotorLeft.setControl(voltageRequest.withVelocity(target).withEnableFOC(true)); + shooterMotorRight.setControl(voltageRequest.withVelocity(target).withEnableFOC(true)); + } +} -- 2.39.5