diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index e1f5cab3..02bcf758 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -1,6 +1,7 @@ -/* + +/* * Constants.java - */ +*/ /* * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. @@ -529,8 +530,7 @@ public final class TurretRight { new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); - - public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = new OpenLoopRampsConfigs() .withDutyCycleOpenLoopRampPeriod(0.02) .withTorqueOpenLoopRampPeriod(0.02) @@ -566,4 +566,79 @@ public final class TurretRight { .withSensorDirection(SensorDirectionValue.Clockwise_Positive); } + public final class Climb { // TODO: update these values + public static final int CAN_ID = 60; + public static final String BUS = "rio"; + public static final double MOTOR_TO_CANCODER = (7.0 / 1.0); // TODO: Update Reductions and add hook to reduction + public static final double CANCODER_TO_MECHANISM = (7.0 / 1.0); + public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; + + public static final double DRUM_CORE_RADIUS_METERS = 0.02; //TODO: Update with physical values from cad/robot + public static final double ROPE_THICKNESS_METERS = 0.005; + + public static final double SIM_MOI = 0.001; + + public static final double MIN_POSITION_MULTI = 0.8; // rotations + public static final double MAX_POSITION_MULTI = 0.8; // rotations + + public static final double MIN_POSITION_RADS = -2 * Math.PI * MIN_POSITION_MULTI; + public static final double MAX_POSITION_RADS = 2 * Math.PI * MAX_POSITION_MULTI; + + + public static final double EPSILON_RADS = Units.degreesToRadians(2.0); + public static final double JOYSTICK_DEADZONE = 0.05; + + public static final double STATE_TIMEOUT = 5.0; + + public static final int CANCODER_ID = 0; // TODO: update id + public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; + + public static final TalonFXConfiguration CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(true)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(0.02) + .withTorqueOpenLoopRampPeriod(0.02) + .withVoltageOpenLoopRampPeriod(0.02); + + public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(0.02) + .withTorqueClosedLoopRampPeriod(0.02) + .withVoltageClosedLoopRampPeriod(0.02); + + public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold( + MAX_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor rotations + .withReverseSoftLimitThreshold( + -MIN_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor rotations + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true); + + public static final FeedbackConfigs FEEDBACK_CONFIGS = + new FeedbackConfigs() + .withFeedbackRemoteSensorID(CANCODER_ID) + .withFeedbackRotorOffset(CANCODER_ABSOLUTE_OFFSET) + .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) + .withRotorToSensorRatio(MOTOR_TO_CANCODER) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); + + public static final MagnetSensorConfigs CANCODER_CONFIG = + new MagnetSensorConfigs() + .withMagnetOffset(1.0 * CANCODER_ABSOLUTE_OFFSET) + .withAbsoluteSensorDiscontinuityPoint(1.0) + .withSensorDirection(SensorDirectionValue.Clockwise_Positive); + } } diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 1db9a35a..688c27c2 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -23,6 +23,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.subsystems.SuperShooter.ShooterState; +import org.team5924.frc2026.subsystems.climb.Climb.ClimbState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; @@ -66,6 +67,8 @@ public void resetPose(Pose2d pose) { /* ### Intake ### */ @Getter @Setter private IntakeState intakeState = IntakeState.OFF; + /* ### Climb ### */ + @Getter @Setter private ClimbState climbState = ClimbState.STOW; /* ### Hopper ### */ @Getter @Setter private HopperState hopperState = HopperState.OFF; diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java new file mode 100644 index 00000000..27790f8e --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -0,0 +1,172 @@ +/* + * Climb.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2026.Constants; +import org.team5924.frc2026.RobotState; +import org.team5924.frc2026.subsystems.sensors.BeamBreakIO; +import org.team5924.frc2026.subsystems.sensors.BeamBreakIOInputsAutoLogged; +import org.team5924.frc2026.util.Elastic; +import org.team5924.frc2026.util.Elastic.Notification; +import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; +import org.team5924.frc2026.util.EqualsUtil; +import org.team5924.frc2026.util.LoggedTunableNumber; + +public class Climb extends SubsystemBase { + + private final ClimbIO io; + private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + + @Setter private double input; + + public enum ClimbState { // TODO: Update climb level values - distance not rads!!! + STOW(new LoggedTunableNumber("Climb/Stow", 0)), + OFF(() -> 0.0), + LEVEL_ONE(new LoggedTunableNumber("Climb/LevelOne", 0)), + LEVEL_TWO(new LoggedTunableNumber("Climb/LevelTwo", 0)), + LEVEL_THREE(new LoggedTunableNumber("Climb/LevelThree", 0)), + CLIMB_DOWN(new LoggedTunableNumber("Climb/ClimbDown", 0)), + DEPLOY(new LoggedTunableNumber("Climb/Deploy", 0)), + DROP(new LoggedTunableNumber("Climb/Drop", 0)), + MOVING(() -> 0.0), + // voltage at which the climb subsystem motor moves when controlled by the operator + MANUAL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); + + @Getter private final DoubleSupplier distance; + + ClimbState(DoubleSupplier distance) { + this.distance = distance; + } + } + + @Getter private ClimbState goalState; + + private final Alert climbMotorDisconnected; + private final Notification climbMotorDisconnectedNotification; + private boolean wasClimbMotorConnected = true; + + private double lastStateChange = 0.0; + + // Climb Beam Break + private final BeamBreakIO beamBreakIO; + private final BeamBreakIOInputsAutoLogged beamBreakInputs = new BeamBreakIOInputsAutoLogged(); + + public Climb(ClimbIO io, BeamBreakIO beamBreakIO) { + this.io = io; + this.goalState = ClimbState.STOW; + this.climbMotorDisconnected = + new Alert("Climb System Motor Disconnected!", Alert.AlertType.kWarning); + this.climbMotorDisconnectedNotification = + new Notification(NotificationLevel.WARNING, "Climb System Motor Disconnected", ""); + this.beamBreakIO = beamBreakIO; + } + + @Override + public void periodic() { + io.periodicUpdates(); + io.updateInputs(inputs); + beamBreakIO.updateInputs(beamBreakInputs); + Logger.processInputs("Climb", inputs); + Logger.processInputs("Climb/BeamBreak", beamBreakInputs); + + Logger.recordOutput("Climb/GoalState", goalState.toString()); + Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState().toString()); + Logger.recordOutput("Climb/TargetRads", distanceToRadians(goalState.distance.getAsDouble())); + + climbMotorDisconnected.set(!inputs.climbMotorConnected); + + handleCurrentState(); + + if (!inputs.climbMotorConnected && wasClimbMotorConnected) { + Elastic.sendNotification(climbMotorDisconnectedNotification); + } + + wasClimbMotorConnected = inputs.climbMotorConnected; + } + + public boolean isAtSetpoint() { + return RobotState.getTime() - lastStateChange > Constants.Climb.STATE_TIMEOUT + || EqualsUtil.epsilonEquals( + inputs.setpointRads, inputs.climbPositionRads, Constants.Climb.EPSILON_RADS); + } + + private void handleCurrentState() { + switch (RobotState.getInstance().getClimbState()) { + case MOVING -> { + io.setPosition(distanceToRadians(goalState.distance.getAsDouble())); + if (isAtSetpoint()) RobotState.getInstance().setClimbState(goalState); + } + case MANUAL -> handleManualState(); + case OFF -> io.stop(); + default -> io.setPosition(distanceToRadians(goalState.distance.getAsDouble())); + } + } + + private void handleManualState() { + if (!goalState.equals(ClimbState.MANUAL)) return; + + if (Math.abs(input) <= Constants.Climb.JOYSTICK_DEADZONE) { + io.runVolts(0); + return; + } + + io.runVolts(ClimbState.MANUAL.getDistance().getAsDouble() * input); + } + + public void setGoalState(ClimbState goalState) { + switch (goalState) { + case MANUAL: + RobotState.getInstance().setClimbState(ClimbState.MANUAL); + break; + case OFF: + RobotState.getInstance().setClimbState(ClimbState.OFF); + break; + case MOVING: + DriverStation.reportError( + "Climb: MOVING is an invalid goal state; it is a transition state!!", null); + return; + default: + RobotState.getInstance().setClimbState(ClimbState.MOVING); + break; + } + this.goalState = goalState; + + lastStateChange = RobotState.getTime(); + } + + private double distanceToRadians(double distance) { + double r = Constants.Climb.DRUM_CORE_RADIUS_METERS; + double t = Constants.Climb.ROPE_THICKNESS_METERS; + + double a = t / (4.0 * Math.PI); + double b = r; + double c = -distance; + + double discriminant = b * b - 4 * a * c; + + if (discriminant < 0) return 0; + + return (-b + Math.sqrt(discriminant)) / (2 * a); + } +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java new file mode 100644 index 00000000..545a101f --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java @@ -0,0 +1,70 @@ +/* + * ClimbIO.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimbIO { + @AutoLog + public static class ClimbIOInputs { + public boolean climbMotorConnected = true; + public double climbPosition = 0.0; + public double climbPositionRads = 0.0; + public double climbPositionCancoder = 0.0; + public double climbVelocityRadsPerSec = 0.0; + public double climbAppliedVoltage = 0.0; + public double climbSupplyCurrentAmps = 0.0; + public double climbTorqueCurrentAmps = 0.0; + public double climbTempCelsius = 0.0; + + public double motionMagicVelocityTarget = 0.0; + public double motionMagicPositionTarget = 0.0; + + public double setpointRads = 0.0; + public double acceleration = 0.0; + + public boolean cancoderConnected = true; + public double cancoderAbsolutePosition = 0.0; + public double cancoderVelocity = 0.0; + public double cancoderSupplyVoltage = 0.0; + public double cancoderPositionRotations = 0.0; + } + + /** + * Updates the inputs object with the latest data from hardware + * + * @param inputs Inputs to update + */ + public default void updateInputs(ClimbIOInputs inputs) {} + + /** Updates that are be called in climb periodic */ + public default void periodicUpdates() {} + + /** + * Sets the subsystem motor to the specified voltage + * + * @param volts number of volts + */ + public default void runVolts(double volts) {} + + public default void setPosition(double rads) {} + + public default void holdPosition(double rads) {} + + /** stops the motor */ + default void stop() {} +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java new file mode 100644 index 00000000..303f8d0f --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -0,0 +1,328 @@ +/* + * ClimbIOTalonFX.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2026.Constants; +import org.team5924.frc2026.util.Elastic; +import org.team5924.frc2026.util.Elastic.Notification; +import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; +import org.team5924.frc2026.util.LoggedTunableNumber; + +public class ClimbIOTalonFX implements ClimbIO { + private final TalonFX climbTalon; + private final CANcoder climbCANCoder; + + private final TalonFXConfigurator climbTalonConfig; + + private final Slot0Configs slot0Configs; + private final MotionMagicConfigs motionMagicConfigs; + private double setpointRads; + + private final LoggedTunableNumber kA = new LoggedTunableNumber("Climb/kA", 0.00); + private final LoggedTunableNumber kS = new LoggedTunableNumber("Climb/kS", 0.13); + private final LoggedTunableNumber kV = new LoggedTunableNumber("Climb/kV", 0.4); + private final LoggedTunableNumber kP = new LoggedTunableNumber("Climb/kP", 6.0); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Climb/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Climb/kD", 0.07); + private final LoggedTunableNumber kG = new LoggedTunableNumber("Climb/kG", 0.33); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("Climb/MotionCruiseVelocity", 90.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("Climb/MotionAcceleration", 900.0); + private final LoggedTunableNumber motionJerk = new LoggedTunableNumber("Climb/MotionJerk", 0.0); + + private final StatusSignal climbPosition; + private final StatusSignal climbVelocity; + private final StatusSignal climbAppliedVoltage; + private final StatusSignal climbSupplyCurrent; + private final StatusSignal climbTorqueCurrent; + private final StatusSignal climbTempCelsius; + + private final StatusSignal cancoderAbsolutePosition; + private final StatusSignal cancoderVelocity; + private final StatusSignal cancoderSupplyVoltage; + private final StatusSignal cancoderPositionRotations; + + private final StatusSignal closedLoopReferenceSlope; + private double prevClosedLoopReferenceSlope = 0.0; + private double prevReferenceSlopeTimestamp = 0.0; + + private final double cancoderToMechanism; + private final double motorToMechanism; + private final double minPositionRads; + private final double maxPositionRads; + + // Single shot for voltage mode, robot loop will call continuously + private final VoltageOut voltageOut; + private final PositionVoltage positionOut; + private final MotionMagicVoltage motionMagicVoltage; + + public ClimbIOTalonFX() { + cancoderToMechanism = Constants.Climb.CANCODER_TO_MECHANISM; + motorToMechanism = Constants.Climb.MOTOR_TO_MECHANISM; + minPositionRads = Constants.Climb.MIN_POSITION_RADS; + maxPositionRads = Constants.Climb.MAX_POSITION_RADS; + + climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); + climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID, new CANBus(Constants.Climb.BUS)); + + climbTalonConfig = climbTalon.getConfigurator(); + + slot0Configs = new Slot0Configs(); + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); + slot0Configs.kG = kG.get(); + + motionMagicConfigs = new MotionMagicConfigs(); + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); + + // Apply Configs + StatusCode[] statusArray = new StatusCode[8]; + + statusArray[0] = climbTalonConfig.apply(Constants.Climb.CONFIG); + statusArray[1] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); + statusArray[2] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); + statusArray[3] = climbTalonConfig.apply(Constants.Climb.SOFTWARE_LIMIT_CONFIGS); + statusArray[4] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); + statusArray[5] = climbTalonConfig.apply(motionMagicConfigs); + statusArray[6] = climbTalonConfig.apply(slot0Configs); + statusArray[7] = + climbCANCoder + .getConfigurator() + .apply(new CANcoderConfiguration().withMagnetSensor(Constants.Climb.CANCODER_CONFIG)); + + boolean isErrorPresent = false; + for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; + + if (isErrorPresent) + Elastic.sendNotification( + new Notification(NotificationLevel.WARNING, "Climb Configs", "Error in climb configs!")); + + Logger.recordOutput("Climb/InitConfReport", statusArray); + + // Get select status signals and set update frequency + climbPosition = climbTalon.getPosition(); + climbVelocity = climbTalon.getVelocity(); + climbAppliedVoltage = climbTalon.getMotorVoltage(); + climbSupplyCurrent = climbTalon.getSupplyCurrent(); + climbTorqueCurrent = climbTalon.getTorqueCurrent(); + climbTempCelsius = climbTalon.getDeviceTemp(); + + cancoderAbsolutePosition = climbCANCoder.getAbsolutePosition(); + cancoderVelocity = climbCANCoder.getVelocity(); + cancoderSupplyVoltage = climbCANCoder.getSupplyVoltage(); + cancoderPositionRotations = climbCANCoder.getPosition(); + + closedLoopReferenceSlope = climbTalon.getClosedLoopReferenceSlope(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, + climbPosition, + climbVelocity, + climbAppliedVoltage, + climbSupplyCurrent, + climbTorqueCurrent, + climbTempCelsius, + cancoderAbsolutePosition, + cancoderVelocity, + cancoderSupplyVoltage, + cancoderPositionRotations, + closedLoopReferenceSlope); + + voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); + positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true).withSlot(0); + motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true).withSlot(0); + + BaseStatusSignal.waitForAll(0.5, cancoderAbsolutePosition); + climbTalon.setPosition(0); + climbCANCoder.setPosition(0.0); // TODO: Check if this value will always be zero + } + + @Override + public void updateInputs(ClimbIOInputs inputs) { + inputs.climbMotorConnected = + BaseStatusSignal.refreshAll( + climbPosition, + climbVelocity, + climbAppliedVoltage, + climbSupplyCurrent, + climbTorqueCurrent, + climbTempCelsius, + closedLoopReferenceSlope) + .isOK(); + + inputs.cancoderConnected = + BaseStatusSignal.refreshAll( + cancoderAbsolutePosition, + cancoderVelocity, + cancoderSupplyVoltage, + cancoderPositionRotations) + .isOK(); + inputs.climbPosition = + BaseStatusSignal.getLatencyCompensatedValueAsDouble(climbPosition, climbVelocity); + inputs.climbPositionRads = Units.rotationsToRadians(climbPosition.getValueAsDouble()); + inputs.climbVelocityRadsPerSec = Units.rotationsToRadians(climbVelocity.getValueAsDouble()); + inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); + inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); + inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); + inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); + + inputs.motionMagicVelocityTarget = + motorPositionToRads(climbTalon.getClosedLoopReferenceSlope().getValueAsDouble()); + inputs.motionMagicPositionTarget = + motorPositionToRads(climbTalon.getClosedLoopReference().getValueAsDouble()); + + inputs.setpointRads = setpointRads; + + double currentTime = closedLoopReferenceSlope.getTimestamp().getTime(); + double timeDiff = currentTime - prevReferenceSlopeTimestamp; + if (timeDiff > 0.0) { + inputs.acceleration = + (inputs.motionMagicVelocityTarget - prevClosedLoopReferenceSlope) / timeDiff; + } + prevClosedLoopReferenceSlope = inputs.motionMagicVelocityTarget; + prevReferenceSlopeTimestamp = currentTime; + + inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); + inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); + inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); + inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); + + inputs.climbPositionCancoder = + Units.rotationsToRadians(inputs.cancoderPositionRotations) / cancoderToMechanism; + } + + @Override + public void periodicUpdates() { + updateLoggedTunableNumbers(); + } + + private void updateLoggedTunableNumbers() { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); + slot0Configs.kG = kG.get(); + + StatusCode statusCode = climbTalon.getConfigurator().apply(slot0Configs); + if (!statusCode.isOK()) { + Elastic.sendNotification( + new Notification( + NotificationLevel.WARNING, + "Climb Slot 0 Configs", + "Error in periodically updating climb Slot0 configs!")); + + Logger.recordOutput("Climb/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA, + kG); + + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); + + StatusCode statusCode = climbTalon.getConfigurator().apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Elastic.sendNotification( + new Notification( + NotificationLevel.WARNING, + "Climb Motion Magic Configs", + "Error in periodically updating climb MotionMagic configs!")); + + Logger.recordOutput("Climb/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); + } + + @Override + public void runVolts(double volts) { + climbTalon.setControl(voltageOut.withOutput(volts)); + } + + @Override + public void stop() { + climbTalon.stopMotor(); + } + + @Override + public void setPosition(double rads) { + setpointRads = clampRads(rads); + climbTalon.setControl(motionMagicVoltage.withPosition(radsToMotorPosition(setpointRads))); + } + + @Override + public void holdPosition(double rads) { + climbTalon.setControl(positionOut.withPosition(radsToMotorPosition(rads))); + } + + private double clampRads(double rads) { + return MathUtil.clamp(rads, minPositionRads, maxPositionRads); + } + + private double radsToMotorPosition(double rads) { + return Units.radiansToRotations(rads * motorToMechanism); + } + + private double motorPositionToRads(double motorPosition) { + return Units.rotationsToRadians(motorPosition / motorToMechanism); + } +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 1f6ce905..45a9376c 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -384,5 +384,4 @@ private double radsToMotorPosition(double rads) { private double motorPositionToRads(double motorPosition) { return Units.rotationsToRadians(motorPosition) / motorToMechanism / mechanismRangePercent; } - }