diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 08319bf..ce9e483 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,9 +58,10 @@ import frc.robot.subsystems.intake.IntakeArmIO; import frc.robot.subsystems.intake.IntakeArmIOReal; import frc.robot.subsystems.intake.IntakeArmIOSim; -import frc.robot.subsystems.intake.IntakeRollerIO; -import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX; -import frc.robot.subsystems.intake.IntakeRollerIOTalonFX; +import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; +import frc.robot.subsystems.intake.RollerIO; +import frc.robot.subsystems.intake.RollerIOSimTalonFX; +import frc.robot.subsystems.intake.RollerIOTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIOTalonFX; @@ -159,7 +160,11 @@ public Robot() { new TurretIOSpark(), new FlywheelIOTalonFX(), new HoodIOSpark()); - intake = new Intake(new IntakeRollerIOTalonFX(), new IntakeArmIOReal()); + intake = + new Intake( + new RollerIOTalonFX(RollerConstants.upperRollerConfig), + new RollerIOTalonFX(RollerConstants.lowerRollerConfig), + new IntakeArmIOReal()); // hopper = new Hopper(new HopperIOReal()); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); SmartDashboard.putData(new Compressor(PneumaticsModuleType.REVPH)); @@ -197,7 +202,11 @@ public Robot() { new FlywheelIOSimTalonFX(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); - intake = new Intake(new IntakeRollerIOSimTalonFX(), new IntakeArmIOSim()); + intake = + new Intake( + new RollerIOSimTalonFX(RollerConstants.upperRollerConfig), + new RollerIOSimTalonFX(RollerConstants.lowerRollerConfig), + new IntakeArmIOSim()); // hopper = new Hopper(new HopperIOSim()); break; @@ -232,7 +241,7 @@ public Robot() { new TurretIO() {}, new FlywheelIO() {}, new HoodIO() {}); - intake = new Intake(new IntakeRollerIO() {}, new IntakeArmIO() {}); + intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}); // hopper = new Hopper(new HopperIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 5748940..de144a6 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -13,34 +13,41 @@ import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { - private final IntakeRollerIO intakeRollerIO; + private final RollerIO upperRollerIO; + private final RollerIO lowerRollerIO; private final IntakeArmIO intakeArmIO; - private final IntakeRollerIOInputsAutoLogged intakeRollerInputs = - new IntakeRollerIOInputsAutoLogged(); + private final RollerIOInputsAutoLogged upperRollerInputs = new RollerIOInputsAutoLogged(); + private final RollerIOInputsAutoLogged lowerRollerInputs = new RollerIOInputsAutoLogged(); private final IntakeArmIOInputsAutoLogged intakeArmInputs = new IntakeArmIOInputsAutoLogged(); - private final Alert disconnectedAlert; + private final Alert upperRollerDisconnectedAlert; + private final Alert lowerRollerDisconnectedAlert; - public Intake(IntakeRollerIO intakeRollerIO, IntakeArmIO intakeArmIO) { - this.intakeRollerIO = intakeRollerIO; + public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intakeArmIO) { + this.upperRollerIO = upperRollerIO; + this.lowerRollerIO = lowerRollerIO; this.intakeArmIO = intakeArmIO; - disconnectedAlert = new Alert("Disconnected intake motor", AlertType.kError); + upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError); + lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); } @Override public void periodic() { long t0 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0; - intakeRollerIO.updateInputs(intakeRollerInputs); + upperRollerIO.updateInputs(upperRollerInputs); + lowerRollerIO.updateInputs(lowerRollerInputs); intakeArmIO.updateInputs(intakeArmInputs); long t1 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0; - Logger.processInputs("IntakeRoller", intakeRollerInputs); + Logger.processInputs("UpperRoller", upperRollerInputs); + Logger.processInputs("LowerRoller", lowerRollerInputs); Logger.processInputs("IntakeArm", intakeArmInputs); long t2 = Constants.PROFILING_ENABLED ? System.nanoTime() : 0; - disconnectedAlert.set(!intakeRollerInputs.connected); + upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); + lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); // Profiling output if (Constants.PROFILING_ENABLED) { @@ -59,7 +66,8 @@ public void periodic() { } public void stop() { - intakeRollerIO.setOpenLoop(Volts.of(0.0)); + upperRollerIO.setOpenLoop(Volts.of(0.0)); + lowerRollerIO.setOpenLoop(Volts.of(0.0)); intakeArmIO.retract(); } @@ -85,7 +93,12 @@ public Command getDeployCommand() { Commands.runOnce(this::deployArm, this), this.idle().withTimeout(0.5), Commands.startEnd( - () -> intakeRollerIO.setVelocity(MetersPerSecond.of(6.0)), () -> {}, this)) + () -> { + upperRollerIO.setVelocity(MetersPerSecond.of(6.0)); + lowerRollerIO.setVelocity(MetersPerSecond.of(6.0)); + }, + () -> {}, + this)) .withName("Intake"); } @@ -94,7 +107,12 @@ public Command getReverseCommand() { Commands.runOnce(this::deployArm, this), this.idle().withTimeout(0.5), Commands.startEnd( - () -> intakeRollerIO.setVelocity(MetersPerSecond.of(-4.0)), () -> {}, this)) + () -> { + upperRollerIO.setVelocity(MetersPerSecond.of(-4.0)); + lowerRollerIO.setVelocity(MetersPerSecond.of(-4.0)); + }, + () -> {}, + this)) .withName("Reverse"); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 392fbf0..474fed6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -2,15 +2,17 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.MotorConstants.KrakenX60Constants; public class IntakeConstants { - public class IntakeRoller { + public static class RollerConstants { public static final Distance rollerRadius = Inches.of(0.85); // motor controller @@ -27,5 +29,23 @@ public class IntakeRoller { // simulation public static final DCMotor gearbox = DCMotor.getKrakenX60(2); + + // configs + public static final RollerConfig upperRollerConfig = + new RollerConfig(CAN2.intakeRollerUpper, CAN2.bus, true); + public static final RollerConfig lowerRollerConfig = + new RollerConfig(CAN2.intakeRollerLower, CAN2.bus, true); + } + + public static class RollerConfig { + public final int port; + public final CANBus bus; + public final boolean inverted; + + public RollerConfig(int port, CANBus bus, boolean inverted) { + this.port = port; + this.bus = bus; + this.inverted = inverted; + } } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java deleted file mode 100644 index 24c003f..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIO.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface IntakeRollerIO { - @AutoLog - public static class IntakeRollerIOInputs { - public boolean connected = false; - public double lowerVelocityMetersPerSec = 0.0; - public double lowerAppliedVolts = 0.0; - public double lowerCurrentAmps = 0.0; - public double upperVelocityMetersPerSec = 0.0; - public double upperAppliedVolts = 0.0; - public double upperCurrentAmps = 0.0; - } - - public default void updateInputs(IntakeRollerIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setVelocity(LinearVelocity tangentialVelocity) {} -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java deleted file mode 100644 index 9a74312..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java +++ /dev/null @@ -1,113 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.intake.IntakeConstants.IntakeRoller.*; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class IntakeRollerIOSimTalonFX implements IntakeRollerIO { - - private final DCMotorSim intakeRollerSim; - - private final TalonFX intakeMotorLeader; - private final TalonFX intakeMotorFollower; - private final TalonFXConfiguration config; - private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - // new VelocityTorqueCurrentFOC(0.0); - - // Inputs from intake motor - private final StatusSignal intakeVelocity; - private final StatusSignal intakeAppliedVolts; - private final StatusSignal intakeCurrent; - - public IntakeRollerIOSimTalonFX() { - intakeMotorLeader = new TalonFX(CAN2.intakeRollerLower, CAN2.bus); - intakeMotorFollower = new TalonFX(CAN2.intakeRollerUpper, CAN2.bus); - config = new TalonFXConfiguration(); - config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - tryUntilOk(5, () -> intakeMotorLeader.getConfigurator().apply(config, 0.25)); - tryUntilOk(5, () -> intakeMotorFollower.getConfigurator().apply(config, 0.25)); - - var intakeMotorSim = intakeMotorLeader.getSimState(); - intakeMotorSim.Orientation = ChassisReference.Clockwise_Positive; - intakeMotorSim.setMotorType(MotorType.KrakenX60); - - intakeRollerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, 0.0005, motorReduction), gearbox); - - intakeVelocity = intakeMotorLeader.getVelocity(); - intakeAppliedVolts = intakeMotorLeader.getMotorVoltage(); - intakeCurrent = intakeMotorLeader.getStatorCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, intakeVelocity, intakeAppliedVolts, intakeCurrent); - - intakeMotorFollower.setControl( - new Follower(CAN2.intakeRollerLower, MotorAlignmentValue.Opposed)); - } - - @Override - public void updateInputs(IntakeRollerIOInputs inputs) { - inputs.connected = - connectedDebounce.calculate( - BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK()); - - // Update simulation state - var intakeMotorSim = intakeMotorLeader.getSimState(); - intakeMotorSim.setSupplyVoltage(RobotConstants.kNominalVoltage); - intakeRollerSim.setInput(intakeMotorSim.getMotorVoltage()); - intakeRollerSim.update(Robot.defaultPeriodSecs); - intakeMotorSim.setRawRotorPosition( - intakeRollerSim.getAngularPositionRotations() * motorReduction); - intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocity().times(motorReduction)); - - inputs.lowerAppliedVolts = intakeAppliedVolts.getValueAsDouble(); - inputs.lowerCurrentAmps = intakeCurrent.getValueAsDouble(); - inputs.lowerVelocityMetersPerSec = - intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; - } - - @Override - public void setOpenLoop(Voltage volts) { - intakeMotorLeader.setControl(voltageRequest.withOutput(volts)); - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - intakeMotorLeader.setControl( - velocityVoltageRequest.withVelocity( - RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) - * motorReduction - / rollerRadius.in(Meters)))); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java deleted file mode 100644 index 8d94d34..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java +++ /dev/null @@ -1,149 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.intake.IntakeConstants.IntakeRoller.*; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; - -public class IntakeRollerIOTalonFX implements IntakeRollerIO { - private final TalonFX intakeMotorLower; - private final TalonFX intakeMotorUpper; - private final TalonFXConfiguration config; - private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - private final VoltageOut voltageRequest = new VoltageOut(0); - // private final VelocityVoltage velocityVoltageRequest = - // new VelocityVoltage(0.0).withSlot(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0).withSlot(1); - private final NeutralOut brake = new NeutralOut(); - - private final TrapezoidProfile profile = - new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAcceleration, maxJerk)); - - // Inputs from intake motor - private final StatusSignal lowerVelocity, upperVelocity; - private final StatusSignal lowerAcceleration, upperAcceleration; - private final StatusSignal lowerAppliedVolts, upperAppliedVolts; - private final StatusSignal lowerCurrent, upperCurrent; - private final StatusSignal lowerDutyCycle, upperDutyCycle; - private final StatusSignal lowerTorqueCurrent, upperTorqueCurrent; - - public IntakeRollerIOTalonFX() { - intakeMotorLower = new TalonFX(CAN2.intakeRollerLower, CAN2.bus); - intakeMotorUpper = new TalonFX(CAN2.intakeRollerUpper, CAN2.bus); - config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; - tryUntilOk(5, () -> intakeMotorLower.getConfigurator().apply(config, 0.25)); // -1 tryUntilOkay - tryUntilOk(5, () -> intakeMotorUpper.getConfigurator().apply(config, 0.25)); // -1 tryUntilOkay - - lowerVelocity = intakeMotorLower.getVelocity(); - lowerAcceleration = intakeMotorLower.getAcceleration(); - lowerAppliedVolts = intakeMotorLower.getMotorVoltage(); - lowerCurrent = intakeMotorLower.getSupplyCurrent(); - lowerDutyCycle = intakeMotorLower.getDutyCycle(); - lowerTorqueCurrent = intakeMotorLower.getTorqueCurrent(); - - upperVelocity = intakeMotorUpper.getVelocity(); - upperAcceleration = intakeMotorUpper.getAcceleration(); - upperAppliedVolts = intakeMotorUpper.getMotorVoltage(); - upperCurrent = intakeMotorUpper.getSupplyCurrent(); - upperDutyCycle = intakeMotorUpper.getDutyCycle(); - upperTorqueCurrent = intakeMotorUpper.getTorqueCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - lowerVelocity, - lowerAcceleration, - lowerAppliedVolts, - lowerCurrent, - lowerDutyCycle, - lowerTorqueCurrent, - upperAcceleration, - upperVelocity, - upperAppliedVolts, - upperCurrent, - upperDutyCycle, - upperTorqueCurrent); - } - - @Override - public void updateInputs(IntakeRollerIOInputs inputs) { - inputs.connected = - connectedDebounce.calculate( - BaseStatusSignal.refreshAll( - lowerVelocity, - lowerAcceleration, - lowerAppliedVolts, - lowerCurrent, - lowerDutyCycle, - lowerTorqueCurrent, - upperAcceleration, - upperVelocity, - upperAppliedVolts, - upperCurrent, - upperDutyCycle, - upperTorqueCurrent) - .isOK()); - - inputs.lowerAppliedVolts = lowerAppliedVolts.getValueAsDouble(); - inputs.lowerCurrentAmps = lowerCurrent.getValueAsDouble(); - inputs.lowerVelocityMetersPerSec = - lowerVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; - inputs.upperAppliedVolts = upperAppliedVolts.getValueAsDouble(); - inputs.upperCurrentAmps = upperCurrent.getValueAsDouble(); - inputs.upperVelocityMetersPerSec = - upperVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; - } - - @Override - public void setOpenLoop(Voltage volts) { - if (volts.in(Volts) < 1e-6) { - intakeMotorLower.setControl(brake); - intakeMotorUpper.setControl(brake); - } else { - intakeMotorLower.setControl(voltageRequest.withOutput(volts)); - intakeMotorUpper.setControl(voltageRequest.withOutput(volts)); - } - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - AngularVelocity angularVelocity = - RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * motorReduction / rollerRadius.in(Meters)); - - // TrapezoidProfile.State goal = - // new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); - // TrapezoidProfile.State setpoint = - // new TrapezoidProfile.State( - // intakeVelocity.getValueAsDouble(), intakeAcceleration.getValueAsDouble()); - - // setpoint = profile.calculate(Robot.defaultPeriodSecs, setpoint, goal); - - // velocityTorqueCurrentRequest.Velocity = setpoint.position; - // velocityTorqueCurrentRequest.Acceleration = setpoint.velocity; - intakeMotorLower.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - intakeMotorUpper.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIO.java b/src/main/java/frc/robot/subsystems/intake/RollerIO.java new file mode 100644 index 0000000..e3cf666 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIO.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.AutoLog; + +public interface RollerIO { + @AutoLog + public static class RollerIOInputs { + public boolean connected = false; + public double velocityMetersPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + } + + public default void updateInputs(RollerIOInputs inputs) {} + + public default void setOpenLoop(Voltage volts) {} + + public default void setVelocity(LinearVelocity tangentialVelocity) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java new file mode 100644 index 0000000..0bc0e0f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -0,0 +1,113 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; +import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; + +public class RollerIOSimTalonFX implements RollerIO { + + private final DCMotorSim rollerSim; + + private final TalonFX motor; + private final TalonFXConfiguration config; + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private final VoltageOut voltageRequest = new VoltageOut(0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0).withSlot(1); + private final NeutralOut brake = new NeutralOut(); + + // Inputs from intake motor + private final StatusSignal velocity; + private final StatusSignal acceleration; + private final StatusSignal appliedVolts; + private final StatusSignal supplyCurrent, torqueCurrent; + private final StatusSignal dutyCycle; + + public RollerIOSimTalonFX(RollerConfig rollerConfig) { + motor = new TalonFX(rollerConfig.port, rollerConfig.bus); + config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = + rollerConfig.inverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); + config.Slot0 = velocityVoltageGains; + config.Slot1 = velocityTorqueCurrentGains; + tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); + + rollerSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.0005, motorReduction), gearbox); + + velocity = motor.getVelocity(); + acceleration = motor.getAcceleration(); + appliedVolts = motor.getMotorVoltage(); + supplyCurrent = motor.getSupplyCurrent(); + dutyCycle = motor.getDutyCycle(); + torqueCurrent = motor.getTorqueCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, velocity, acceleration, appliedVolts, supplyCurrent, dutyCycle, torqueCurrent); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + inputs.connected = + connectedDebounce.calculate( + BaseStatusSignal.refreshAll( + velocity, acceleration, appliedVolts, supplyCurrent, dutyCycle, torqueCurrent) + .isOK()); + + // Update simulation state + var motorSim = motor.getSimState(); + motorSim.setSupplyVoltage(RobotConstants.kNominalVoltage); + rollerSim.setInput(motorSim.getMotorVoltage()); + rollerSim.update(Robot.defaultPeriodSecs); + motorSim.setRawRotorPosition(rollerSim.getAngularPositionRotations() * motorReduction); + motorSim.setRotorVelocity(rollerSim.getAngularVelocity().times(motorReduction)); + + inputs.appliedVolts = appliedVolts.getValueAsDouble(); + inputs.currentAmps = supplyCurrent.getValueAsDouble(); + inputs.velocityMetersPerSec = + velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; + } + + @Override + public void setOpenLoop(Voltage volts) { + if (volts.in(Volts) < 1e-6) { + motor.setControl(brake); + } else { + motor.setControl(voltageRequest.withOutput(volts)); + } + } + + @Override + public void setVelocity(LinearVelocity tangentialVelocity) { + AngularVelocity angularVelocity = + RadiansPerSecond.of( + tangentialVelocity.in(MetersPerSecond) * motorReduction / rollerRadius.in(Meters)); + motor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java new file mode 100644 index 0000000..d72ea1e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -0,0 +1,110 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; + +public class RollerIOTalonFX implements RollerIO { + private final TalonFX motor; + private final TalonFXConfiguration config; + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private final VoltageOut voltageRequest = new VoltageOut(0); + // private final VelocityVoltage velocityVoltageRequest = + // new VelocityVoltage(0.0).withSlot(0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0).withSlot(1); + private final NeutralOut brake = new NeutralOut(); + + // private final TrapezoidProfile profile = + // new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAcceleration, maxJerk)); + + // Inputs from intake motor + private final StatusSignal velocity; + private final StatusSignal acceleration; + private final StatusSignal appliedVolts; + private final StatusSignal supplyCurrent, torqueCurrent; + private final StatusSignal dutyCycle; + + public RollerIOTalonFX(RollerConfig rollerConfig) { + motor = new TalonFX(rollerConfig.port, rollerConfig.bus); + config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = + rollerConfig.inverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); + config.Slot0 = velocityVoltageGains; + config.Slot1 = velocityTorqueCurrentGains; + tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); // -1 tryUntilOkay + + velocity = motor.getVelocity(); + acceleration = motor.getAcceleration(); + appliedVolts = motor.getMotorVoltage(); + supplyCurrent = motor.getSupplyCurrent(); + dutyCycle = motor.getDutyCycle(); + torqueCurrent = motor.getTorqueCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, velocity, acceleration, appliedVolts, supplyCurrent, dutyCycle, torqueCurrent); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + inputs.connected = + connectedDebounce.calculate( + BaseStatusSignal.refreshAll( + velocity, acceleration, appliedVolts, supplyCurrent, dutyCycle, torqueCurrent) + .isOK()); + + inputs.appliedVolts = appliedVolts.getValueAsDouble(); + inputs.currentAmps = supplyCurrent.getValueAsDouble(); + inputs.velocityMetersPerSec = + velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; + } + + @Override + public void setOpenLoop(Voltage volts) { + if (volts.in(Volts) < 1e-6) { + motor.setControl(brake); + } else { + motor.setControl(voltageRequest.withOutput(volts)); + } + } + + @Override + public void setVelocity(LinearVelocity tangentialVelocity) { + AngularVelocity angularVelocity = + RadiansPerSecond.of( + tangentialVelocity.in(MetersPerSecond) * motorReduction / rollerRadius.in(Meters)); + + // TrapezoidProfile.State goal = + // new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); + // TrapezoidProfile.State setpoint = + // new TrapezoidProfile.State( + // intakeVelocity.getValueAsDouble(), intakeAcceleration.getValueAsDouble()); + + // setpoint = profile.calculate(Robot.defaultPeriodSecs, setpoint, goal); + + // velocityTorqueCurrentRequest.Velocity = setpoint.position; + // velocityTorqueCurrentRequest.Acceleration = setpoint.velocity; + motor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + } +}