diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cc95ecb..b37f35f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -103,6 +103,8 @@ public static final class CAN2 { // Intake public static final int INTAKE_ROLLER_LOWER = 22; public static final int INTAKE_ROLLER_UPPER = 23; + public static final int INTAKE_ARM_RIGHT = 27; + public static final int INTAKE_ARM_LEFT = 26; } public static final class CANHD { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d41d099..1dcfcba 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,12 +7,10 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.REVPHSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -33,7 +31,6 @@ import frc.lib.ControllerSelector.DriverConfig; import frc.lib.ControllerSelector.DriverController; import frc.lib.ControllerSelector.OperatorConfig; -import frc.lib.LoggedCompressor; import frc.lib.LoggedPowerDistribution; import frc.lib.ZorroController.Axis; import frc.robot.Constants.CANBusPorts.CAN2; @@ -68,9 +65,10 @@ import frc.robot.subsystems.hopper.HopperIOSim; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeArmIO; -import frc.robot.subsystems.intake.IntakeArmIOReal; -import frc.robot.subsystems.intake.IntakeArmIOSim; +import frc.robot.subsystems.intake.IntakeArmIOSimSpark; +import frc.robot.subsystems.intake.IntakeArmIOSpark; import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.intake.IntakeConstants.ArmConstants; import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; import frc.robot.subsystems.intake.RollerIO; import frc.robot.subsystems.intake.RollerIOSimSpark; @@ -139,8 +137,6 @@ public class Robot extends LoggedRobot { private Intake intake; private Hopper hopper; private LEDController leds = LEDController.getInstance(); - private LoggedCompressor compressor; - private PneumaticsSimulator pneumaticsSimulator; // Battery simulation constants private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc @@ -201,9 +197,9 @@ public Robot() { new Intake( new RollerIOSpark(RollerConstants.UPPER_ROLLER_CONFIG), new RollerIOSpark(RollerConstants.LOWER_ROLLER_CONFIG), - new IntakeArmIOReal()); + new IntakeArmIOSpark(ArmConstants.LEFT_ARM_CONFIG), + new IntakeArmIOSpark(ArmConstants.RIGHT_ARM_CONFIG)); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); - compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor"); // Start kernel log monitoring (singleton, starts automatically on first call) KernelLogMonitor.getInstance(); @@ -239,14 +235,12 @@ public Robot() { new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOSim()); - var intakeArmIOSim = new IntakeArmIOSim(); intake = new Intake( new RollerIOSimSpark(RollerConstants.UPPER_ROLLER_CONFIG), new RollerIOSimSpark(RollerConstants.LOWER_ROLLER_CONFIG), - intakeArmIOSim); - pneumaticsSimulator = - new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1)); + new IntakeArmIOSimSpark(ArmConstants.LEFT_ARM_CONFIG), + new IntakeArmIOSimSpark(ArmConstants.RIGHT_ARM_CONFIG)); break; case REPLAY: // Replaying a log @@ -281,7 +275,9 @@ public Robot() { new FlywheelIO() {}, new HoodIO() {}); if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIO() {}); - intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}); + intake = + new Intake( + new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}, new IntakeArmIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; } @@ -348,7 +344,6 @@ public void robotPeriodic() { logCANBus("CAN2", Constants.CANBusPorts.CAN2.BUS); logCANBus("CANHD", Constants.CANBusPorts.CANHD.BUS); powerDistribution.log(); - if (compressor != null) compressor.log(); logHIDs(); logScheduler(); @@ -437,9 +432,6 @@ public void teleopInit() { public void teleopPeriodic() { leds.displayHubCountdown(); leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning()); - if (!DriverStation.isFMSAttached()) { - leds.displayCompressorState(compressor != null && compressor.isEnabled()); - } } /** This function is called once when test mode is enabled. */ @@ -466,11 +458,8 @@ public void simulationInit() { /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { - // Skip battery simulation during replay (pneumaticsSimulator is only initialized in SIM mode) - if (pneumaticsSimulator == null) return; // Update battery voltage based on total current draw this cycle - pneumaticsSimulator.update(Robot.defaultPeriodSecs); RoboRioSim.setVInVoltage( vBusFilter.calculate( Math.max( @@ -480,7 +469,6 @@ public void simulationPeriodic() { launcher.getSimCurrentDrawAmps(), feeder.getSimCurrentDrawAmps(), intake.getSimCurrentDrawAmps(), - pneumaticsSimulator.getCompressorCurrentAmps(), ELECTRONICS_OVERHEAD_AMPS)))); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e0393be..0b3c40b 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,15 +1,22 @@ package frc.robot.subsystems.intake; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.Robot; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -17,27 +24,58 @@ public class Intake extends SubsystemBase { private final RollerIO upperRollerIO; private final RollerIO lowerRollerIO; - private final IntakeArmIO intakeArmIO; + private final IntakeArmIO leftArmIO; + private final IntakeArmIO rightArmIO; private final RollerIOInputsAutoLogged upperRollerInputs = new RollerIOInputsAutoLogged(); private final RollerIOInputsAutoLogged lowerRollerInputs = new RollerIOInputsAutoLogged(); - private final IntakeArmIOInputsAutoLogged intakeArmInputs = new IntakeArmIOInputsAutoLogged(); + private final IntakeArmIOInputsAutoLogged leftArmInputs = new IntakeArmIOInputsAutoLogged(); + private final IntakeArmIOInputsAutoLogged rightArmInputs = new IntakeArmIOInputsAutoLogged(); private final Alert upperRollerDisconnectedAlert; private final Alert lowerRollerDisconnectedAlert; + private final Alert leftArmDisconnectedAlert; + private final Alert rightArmDisconnectedAlert; + private final Alert armSeedOutOfRangeAlert; + + // Both arms independently follow the same profiled setpoint; commands only ever move the goal. + private final TrapezoidProfile armProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(PROFILE_MAX_VELOCITY, PROFILE_MAX_ACCELERATION)); + private State armGoal = new State(STOWED_POS_RAD, 0.0); + private State armSetpoint = new State(STOWED_POS_RAD, 0.0); + + // Only the left arm's Spark has an absolute encoder wired up. Both arms' relative encoders, + // and the motion profile itself, are seeded from that one reading once it's settled — its + // first CAN frame after connecting can be a stale default rather than a real sample. + private boolean armSeeded = false; + private final LinearFilter armSeedFilter = LinearFilter.movingAverage(ARM_SEED_SETTLE_SAMPLES); + private int armSeedSampleCount = 0; // Injected after both subsystems are created to avoid a circular dependency. // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. private BooleanSupplier hopperIsDeployed; private Supplier hopperDeployCommand; - public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intakeArmIO) { + public Intake( + RollerIO upperRollerIO, + RollerIO lowerRollerIO, + IntakeArmIO leftArmIO, + IntakeArmIO rightArmIO) { this.upperRollerIO = upperRollerIO; this.lowerRollerIO = lowerRollerIO; - this.intakeArmIO = intakeArmIO; + this.leftArmIO = leftArmIO; + this.rightArmIO = rightArmIO; upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError); lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); + leftArmDisconnectedAlert = new Alert("Disconnected left intake arm", AlertType.kError); + rightArmDisconnectedAlert = new Alert("Disconnected right intake arm", AlertType.kError); + armSeedOutOfRangeAlert = + new Alert( + "Intake arm absolute encoder seed is outside the soft limit range — check" + + " absEncoderOffset", + AlertType.kWarning); } @Override @@ -45,18 +83,61 @@ public void periodic() { long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; upperRollerIO.updateInputs(upperRollerInputs); lowerRollerIO.updateInputs(lowerRollerInputs); - intakeArmIO.updateInputs(intakeArmInputs); + leftArmIO.updateInputs(leftArmInputs); + rightArmIO.updateInputs(rightArmInputs); long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("UpperRoller", upperRollerInputs); Logger.processInputs("LowerRoller", lowerRollerInputs); - Logger.processInputs("IntakeArm", intakeArmInputs); + Logger.processInputs("LeftArm", leftArmInputs); + Logger.processInputs("RightArm", rightArmInputs); long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); + leftArmDisconnectedAlert.set(!leftArmInputs.connected); + rightArmDisconnectedAlert.set(!rightArmInputs.connected); Logger.recordOutput("Faults/Intake/UpperRollerDisconnected", !upperRollerInputs.connected); Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected); + Logger.recordOutput("Faults/Intake/LeftArmDisconnected", !leftArmInputs.connected); + Logger.recordOutput("Faults/Intake/RightArmDisconnected", !rightArmInputs.connected); + + // Seed both relative encoders, and the motion profile, from the left arm's absolute encoder + // once it reports connected and its reading has settled. The first CAN frame(s) after + // connecting can be a stale default rather than a real sample, so those are discarded outright + // — never fed to the moving average — and only samples known to be past that go into the + // average the seed actually trusts. The settled reading is clamped into the soft limit range + // so a miscalibrated absEncoderOffset can't seed a position the arm can never leave; + // armSeedOutOfRangeAlert flags when that clamp did something. + if (!armSeeded && leftArmInputs.connected) { + armSeedSampleCount++; + if (armSeedSampleCount > ARM_SEED_DISCARD_SAMPLES) { + double filteredSeedPositionRad = + armSeedFilter.calculate(leftArmInputs.absolutePosition.getRadians()); + if (armSeedSampleCount >= ARM_SEED_DISCARD_SAMPLES + ARM_SEED_SETTLE_SAMPLES) { + double seedPositionRad = MathUtil.clamp(filteredSeedPositionRad, minPosRad, maxPosRad); + armSeedOutOfRangeAlert.set(seedPositionRad != filteredSeedPositionRad); + leftArmIO.resetEncoder(Radians.of(seedPositionRad)); + rightArmIO.resetEncoder(Radians.of(seedPositionRad)); + armGoal = new State(seedPositionRad, 0.0); + armSetpoint = new State(seedPositionRad, 0.0); + armSeeded = true; + } + } + } + + // Advance the arm motion profile and drive both arms to the resulting setpoint. Commands + // never set arm position directly — they only move armGoal, and this is the sole place + // setPosition() is called. Nothing drives the arm until it's seeded: both Sparks hold their + // last commanded state (brake mode, no output) rather than closing a position loop against + // the unseeded, false-zero relative encoder. + if (armSeeded) { + armSetpoint = armProfile.calculate(Robot.defaultPeriodSecs, armSetpoint, armGoal); + leftArmIO.setPosition( + Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + rightArmIO.setPosition( + Radians.of(armSetpoint.position), RadiansPerSecond.of(armSetpoint.velocity)); + } // Profiling output if (Constants.FeatureFlags.PROFILING_ENABLED) { @@ -77,23 +158,22 @@ public void periodic() { public void stop() { upperRollerIO.setOpenLoop(Volts.of(0.0)); lowerRollerIO.setOpenLoop(Volts.of(0.0)); - intakeArmIO.retract(); + armGoal = new State(STOWED_POS_RAD, 0.0); } public void deployArm() { - intakeArmIO.deploy(); + armGoal = new State(DEPLOYED_POS_RAD, 0.0); } public void retractArm() { - intakeArmIO.retract(); - } - - public Boolean isDeployed() { - return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kForward; + armGoal = new State(STOWED_POS_RAD, 0.0); } + /** True once both arms are seeded and measured within tolerance of the stowed position. */ public boolean isStowed() { - return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kReverse; + return armSeeded + && MathUtil.isNear(STOWED_POS_RAD, leftArmInputs.positionRad, STOWED_TOLERANCE_RAD) + && MathUtil.isNear(STOWED_POS_RAD, rightArmInputs.positionRad, STOWED_TOLERANCE_RAD); } /** @@ -135,7 +215,10 @@ public Command getDeployCommand() { /** Returns the total motor current draw for battery simulation. */ public double getSimCurrentDrawAmps() { - return upperRollerInputs.currentAmps + lowerRollerInputs.currentAmps; + return upperRollerInputs.currentAmps + + lowerRollerInputs.currentAmps + + leftArmInputs.currentAmps + + rightArmInputs.currentAmps; } public Command getReverseCommand() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java index 4171e42..bee116d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java @@ -1,17 +1,25 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; public interface IntakeArmIO { @AutoLog public static class IntakeArmIOInputs { - public DoubleSolenoid.Value isDeployed = DoubleSolenoid.Value.kReverse; + public boolean connected = false; + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; + + public Rotation2d absolutePosition = Rotation2d.kZero; } public default void updateInputs(IntakeArmIOInputs inputs) {} - public default void deploy() {} + public default void setPosition(Angle rotation, AngularVelocity velocity) {} - public default void retract() {} + public default void resetEncoder(Angle position) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java deleted file mode 100644 index 6cfdefb..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; - -public class IntakeArmIOReal implements IntakeArmIO { - public final DoubleSolenoid intakeArmPneumatic; - - public IntakeArmIOReal() { - intakeArmPneumatic = - new DoubleSolenoid(PneumaticsModuleType.REVPH, INTAKE_ARM_FWD, INTAKE_ARM_REV); - } - - @Override - public void updateInputs(IntakeArmIOInputs inputs) { - inputs.isDeployed = intakeArmPneumatic.get(); - } - - @Override - public void deploy() { - intakeArmPneumatic.set(kForward); - } - - @Override - public void retract() { - intakeArmPneumatic.set(kReverse); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java deleted file mode 100644 index f7bebbd..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; - -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.simulation.DoubleSolenoidSim; - -public class IntakeArmIOSim implements IntakeArmIO { - public final DoubleSolenoidSim intakeArmPneumatic; - - public IntakeArmIOSim() { - intakeArmPneumatic = - new DoubleSolenoidSim(PneumaticsModuleType.REVPH, INTAKE_ARM_FWD, INTAKE_ARM_REV); - intakeArmPneumatic.set(kReverse); - } - - @Override - public void updateInputs(IntakeArmIOInputs inputs) { - inputs.isDeployed = intakeArmPneumatic.get(); - } - - @Override - public void deploy() { - intakeArmPneumatic.set(kForward); - } - - @Override - public void retract() { - intakeArmPneumatic.set(kReverse); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java new file mode 100644 index 0000000..460fdb7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSimSpark.java @@ -0,0 +1,123 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants.MotorConstants.NEOConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; + +public class IntakeArmIOSimSpark implements IntakeArmIO { + private static final double kP = 1.0; + private static final double kD = 1.0; + + private final SingleJointedArmSim armSim; + + private final SparkMax motor; + private final SparkClosedLoopController controller; + private final SparkMaxSim motorSim; + private final boolean hasAbsoluteEncoder; + + private final SparkMaxConfig motorConfig; + + public IntakeArmIOSimSpark(ArmConfig armConfig) { + motor = new SparkMax(armConfig.port(), MotorType.kBrushless); + hasAbsoluteEncoder = armConfig.hasAbsoluteEncoder(); + + controller = motor.getClosedLoopController(); + + motorConfig = new SparkMaxConfig(); + + motorConfig + .inverted(armConfig.inverted()) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); + + motorConfig + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + motorConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); + + motorConfig + .softLimit + .forwardSoftLimit(maxPosRad) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(minPosRad) + .reverseSoftLimitEnabled(true); + + motor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + motorSim = new SparkMaxSim(motor, gearbox); + + // Starts stowed (maxPosRad), matching where the real arm sits at boot. + armSim = + new SingleJointedArmSim( + gearbox, + motorReduction, + MOMENT_OF_INERTIA_KG_M2, + ARM_LENGTH_METERS, + minPosRad, + maxPosRad, + true, + maxPosRad); + + motorSim.setPosition(maxPosRad); + } + + @Override + public void updateInputs(IntakeArmIOInputs inputs) { + // Update simulation state + double busVoltage = RoboRioSim.getVInVoltage(); + armSim.setInputVoltage(motorSim.getAppliedOutput() * busVoltage); + armSim.update(Robot.defaultPeriodSecs); + + motorSim.iterate(armSim.getVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); + + // Update inputs + inputs.connected = true; + inputs.positionRad = motorSim.getPosition(); + inputs.velocityRadPerSec = motorSim.getVelocity(); + inputs.appliedVolts = motorSim.getAppliedOutput() * motorSim.getBusVoltage(); + inputs.currentAmps = Math.abs(motorSim.getMotorCurrent()); + + if (hasAbsoluteEncoder) { + // No offset/calibration to simulate — the sim arm's position is already ground truth, so + // it's reported directly, matching the real absolute encoder's convention. + inputs.absolutePosition = new Rotation2d(motorSim.getPosition()); + } + } + + @Override + public void setPosition(Angle rotation, AngularVelocity velocity) { + double feedforward = + RobotConstants.NOMINAL_VOLTAGE + * velocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond) + + kG * Math.cos(motorSim.getPosition()); + controller.setSetpoint( + rotation.magnitude(), ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); + } + + @Override + public void resetEncoder(Angle position) { + motorSim.setPosition(position.magnitude()); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java new file mode 100644 index 0000000..b9d9e0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSpark.java @@ -0,0 +1,134 @@ +package frc.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.intake.IntakeConstants.ArmConstants.*; +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.AbsoluteEncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.MotorConstants.NEOConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.util.SparkOdometryThread; +import frc.robot.util.SparkOdometryThread.SparkInputs; + +public class IntakeArmIOSpark implements IntakeArmIO { + private static final double kPPos = 1.0; + + private final SparkMax motor; + private final AbsoluteEncoder absEncoder; + private final RelativeEncoder relEncoder; + private final SparkClosedLoopController controller; + private final SparkInputs sparkInputs; + + private final SparkMaxConfig motorConfig; + private final AbsoluteEncoderConfig absEncoderConfig; + + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public IntakeArmIOSpark(ArmConfig armConfig) { + motor = new SparkMax(armConfig.port(), MotorType.kBrushless); + absEncoder = armConfig.hasAbsoluteEncoder() ? motor.getAbsoluteEncoder() : null; + relEncoder = motor.getEncoder(); + controller = motor.getClosedLoopController(); + + absEncoderConfig = new AbsoluteEncoderConfig(); + + if (armConfig.hasAbsoluteEncoder()) { + // AbsoluteEncoderConfig.inverted() only applies in brushed mode (see its javadoc); this + // is a brushless NEO. + absEncoderConfig + .zeroOffset(absEncoderOffset) + .positionConversionFactor(absEncoderPositionFactor) + .velocityConversionFactor(absEncoderVelocityFactor); + } + + motorConfig = new SparkMaxConfig(); + + motorConfig + .inverted(armConfig.inverted()) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEOConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); + + motorConfig + .encoder + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + if (armConfig.hasAbsoluteEncoder()) { + motorConfig.absoluteEncoder.apply(absEncoderConfig); + } + + motorConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(kPPos, 0.0, 0.0, ClosedLoopSlot.kSlot0); + + motorConfig + .softLimit + .forwardSoftLimit(maxPosRad) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(minPosRad) + .reverseSoftLimitEnabled(true); + + motorConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); + + tryUntilOk( + motor, + 5, + () -> + motor.configure( + motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + sparkInputs = SparkOdometryThread.getInstance().registerSpark(motor, relEncoder); + } + + @Override + public void updateInputs(IntakeArmIOInputs inputs) { + inputs.positionRad = sparkInputs.getPosition(); + inputs.velocityRadPerSec = sparkInputs.getVelocity(); + inputs.appliedVolts = sparkInputs.getAppliedVolts(); + inputs.currentAmps = sparkInputs.getOutputCurrent(); + inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); + + if (absEncoder != null) { + // The absolute encoder is a separate physical sensor from the relative encoder, so + // armConfig.inverted() (which flips the relative encoder's sign, along with motor output) + // has no effect on it. Measured on the bench: absEncoder.getPosition() decreases as the + // arm moves from stowed to deployed, matching the relative encoder's convention. + inputs.absolutePosition = new Rotation2d(absEncoder.getPosition()); + } + } + + @Override + public void setPosition(Angle rotation, AngularVelocity velocity) { + double feedforward = + RobotConstants.NOMINAL_VOLTAGE + * velocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond) + + kG * Math.cos(relEncoder.getPosition()); + double setpoint = MathUtil.clamp(rotation.magnitude(), minPosRad, maxPosRad); + controller.setSetpoint(setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforward); + } + + @Override + public void resetEncoder(Angle position) { + relEncoder.setPosition(position.magnitude()); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index c695723..f5d5b2e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -6,6 +6,7 @@ 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 edu.wpi.first.units.measure.LinearVelocity; import frc.robot.Constants.CANBusPorts.CAN2; @@ -51,4 +52,71 @@ public static class TalonConfig { new Slot1Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(2.5); } } + + public static class ArmConstants { + public static final double motorReduction = 15.0; + + public static final double encoderPositionFactor = 2 * Math.PI / motorReduction; + public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * motorReduction); + + public static final double absEncoderPositionFactor = 2 * Math.PI; + public static final double absEncoderVelocityFactor = (2 * Math.PI) / 60.0; + + // Measured on the bench: raw absolute-encoder reading at horizontal (0 rad — see the kG + // feedforward comment below), in rotations. Averaged from two 5-second pauses at horizontal, + // approached from opposite directions, which agreed within ~7° of each other. + public static final double absEncoderOffset = 0.7297; + + // Seed settling. The absolute encoder's first CAN frame(s) after connecting can be a stale + // default rather than a real reading, so those samples are discarded outright — never fed to + // the moving average — before the average starts filling on samples known to be past that. + public static final int ARM_SEED_DISCARD_SAMPLES = 5; // 0.1s @ 50Hz + public static final int ARM_SEED_SETTLE_SAMPLES = 25; // 0.5s @ 50Hz, after the discard + + public static final DCMotor gearbox = DCMotor.getNEO(2); + public static final AngularVelocity maxAngularVelocity = + RadiansPerSecond.of(gearbox.freeSpeedRadPerSec / motorReduction); + + // Measured on the bench with the absolute encoder, relative to horizontal (0): the stowed and + // deployed hardstops, each held for 5 seconds. Deployed sits past horizontal, not at it — the + // gravity feedforward's zero reference is a physical fact about the mechanism, not a hardstop. + // No margin included — these are the exact hand-measured hardstop positions. + public static final double maxPosRad = Degrees.of(81.0).in(Radians); + public static final double minPosRad = Degrees.of(-51.7).in(Radians); + + // Motion profile + public static final double PROFILE_MAX_VELOCITY = maxAngularVelocity.in(RadiansPerSecond); + public static final double PROFILE_MAX_ACCELERATION = 20.0; // rad/s^2 — assumed, tune on robot + public static final double STOWED_TOLERANCE_RAD = + Degrees.of(5.0).in(Radians); // assumed, tune on robot + + // Gravity feedforward. Position 0 is horizontal, so torque from gravity — and thus the + // holding voltage — scales with cos(position) and drops to 0 at vertical (±90°). + public static final double kG = 0.0; // volts — not yet measured, has no effect until tuned + + // Sim physics. SingleJointedArmSim models the arm as a uniform rod pivoting at one end, + // giving it the same cos(position) gravity torque that kG compensates for on the real arm. + public static final double MOMENT_OF_INERTIA_KG_M2 = 0.004; // assumed, tune on robot + + // SingleJointedArmSim hardcodes the center of mass at ARM_LENGTH_METERS / 2, so if the + // real mass is concentrated (e.g. at the roller end) rather than spread evenly like a + // uniform rod, set this to 2x the measured pivot-to-mass-concentration distance so the + // sim's assumed center of mass lands at the real one. + public static final double ARM_LENGTH_METERS = + 0.4; // ~16in — assumed placeholder, tune on robot + + // Target positions. Motors and encoders are mounted inverted, so the raw range is flipped: + // stowed reads as the top of the range and deployed reads as the bottom. + public static final double STOWED_POS_RAD = maxPosRad; + public static final double DEPLOYED_POS_RAD = minPosRad; + + // Configs + // Only the left arm's Spark has an absolute encoder wired up; see Intake's seeding comment. + public record ArmConfig(int port, CANBus bus, boolean inverted, boolean hasAbsoluteEncoder) {} + + public static final ArmConfig LEFT_ARM_CONFIG = + new ArmConfig(CAN2.INTAKE_ARM_LEFT, CAN2.BUS, false, true); + public static final ArmConfig RIGHT_ARM_CONFIG = + new ArmConfig(CAN2.INTAKE_ARM_RIGHT, CAN2.BUS, true, false); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 4a422d2..056a8d4 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -70,7 +70,6 @@ public void updateInputs(RollerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { flex.setVoltage(volts); - ; } @Override