diff --git a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java index d6a3641..e915ac2 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java @@ -41,7 +41,13 @@ public AbsoluteEncoderIOCANCoder( angle = CANCoder.getAbsolutePosition(); - updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(200)); + // Further reduce update frequency for absolute encoder since it's mainly used for + // initialization + updateThread.CTRECheckErrorAndRetry( + () -> angle.setUpdateFrequency(10.0)); // Further reduced from 25.0 + + // Optimize bus utilization + CANCoder.optimizeBusUtilization(1.0); } @Override diff --git a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java index 90c66a2..02a2d54 100644 --- a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java +++ b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java @@ -51,6 +51,13 @@ public DistanceSensorIOCANRange(Device.CAN id, String name, CANrangeConfiguratio ambientSignal = CANRange.getAmbientSignal(); distance = CANRange.getDistance(); + + // Set very low update frequencies for distance sensors (not critical for control) + updateThread.CTRECheckErrorAndRetry( + () -> BaseStatusSignal.setUpdateFrequencyForAll(10.0, ambientSignal, distance)); + + // Optimize bus utilization + CANRange.optimizeBusUtilization(1.0); } @Override diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 83c744e..729f730 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -105,6 +105,7 @@ public MotorIOTalonFX( this.name = name; motor = new TalonFX(main.id(), main.bus()); + motor.optimizeBusUtilization(); updateThread.CTRECheckErrorAndRetry(() -> motor.getConfigurator().apply(config)); // Initialize lists @@ -140,17 +141,38 @@ public MotorIOTalonFX( closedLoopReference = motor.getClosedLoopReference(); closedLoopReferenceSlope = motor.getClosedLoopReferenceSlope(); + // Set different update frequencies based on signal importance + // Critical signals for control (reduced frequency) updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 100, position, velocity, supplyCurrent, supplyCurrent, torqueCurrent, temperature)); + 50.0, position, velocity)); // Critical for control loops + // Important telemetry (lower frequency) updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 200, closedLoopError, closedLoopReference, closedLoopReferenceSlope)); + 20.0, supplyCurrent, torqueCurrent)); // Important for monitoring - motor.optimizeBusUtilization(0, 1.0); + // Non-critical telemetry (very low frequency) + updateThread.CTRECheckErrorAndRetry( + () -> + BaseStatusSignal.setUpdateFrequencyForAll( + 10.0, temperature, supplyVoltage)); // Less critical + + // Control loop feedback (reduced frequency when in closed-loop) + updateThread.CTRECheckErrorAndRetry( + () -> + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, closedLoopError, closedLoopReference, closedLoopReferenceSlope)); + + // Optimize bus utilization with longer timeout for better optimization + motor.optimizeBusUtilization(1.0, 1.0); // Increased timeout for better optimization + + // Optimize follower bus utilization + for (TalonFX follower : followers) { + follower.optimizeBusUtilization(1.0, 1.0); + } } /** @@ -241,19 +263,17 @@ protected ControlType getCurrentControlType() { */ @Override public void updateInputs(MotorInputs inputs) { - inputs.connected = - BaseStatusSignal.refreshAll( - position, - velocity, - supplyVoltage, - supplyCurrent, - torqueCurrent, - temperature, - closedLoopError, - closedLoopReference, - closedLoopReferenceSlope) + // Refresh signals in groups based on priority + // Refresh critical signals first + boolean criticalSignalsOk = BaseStatusSignal.refreshAll(position, velocity).isOK(); + + // Refresh telemetry signals + boolean telemetrySignalsOk = + BaseStatusSignal.refreshAll(supplyCurrent, torqueCurrent, temperature, supplyVoltage) .isOK(); + inputs.connected = criticalSignalsOk && telemetrySignalsOk; + inputs.position = position.getValue(); inputs.velocity = velocity.getValue(); inputs.appliedVoltage = supplyVoltage.getValue(); @@ -261,34 +281,47 @@ public void updateInputs(MotorInputs inputs) { inputs.torqueCurrent = torqueCurrent.getValue(); inputs.temperature = temperature.getValue(); - // Interpret control-loop status signals conditionally based on current mode - Double closedLoopErrorValue = closedLoopError.getValue(); - Double closedLoopTargetValue = closedLoopReference.getValue(); - + // Check current control modes boolean isRunningPositionControl = isRunningPositionControl(); boolean isRunningMotionMagic = isRunningMotionMagic(); boolean isRunningVelocityControl = isRunningVelocityControl(); - inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null; - - inputs.activeTrajectoryPosition = - isRunningPositionControl && isRunningMotionMagic - ? Rotations.of(closedLoopTargetValue) - : null; - - inputs.goalPosition = isRunningPositionControl ? goalPosition : null; - - if (isRunningVelocityControl) { - inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue); - inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue); - } else if (isRunningPositionControl && isRunningMotionMagic) { - var targetVelocity = closedLoopReferenceSlope.getValue(); - inputs.velocityError = - RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond)); - inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity); + // Only update closed-loop signals when in closed-loop modes + if (isRunningPositionControl || isRunningVelocityControl) { + BaseStatusSignal.refreshAll(closedLoopError, closedLoopReference, closedLoopReferenceSlope); + + // Interpret control-loop status signals conditionally based on current mode + Double closedLoopErrorValue = closedLoopError.getValue(); + Double closedLoopTargetValue = closedLoopReference.getValue(); + + inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null; + + inputs.activeTrajectoryPosition = + isRunningPositionControl && isRunningMotionMagic + ? Rotations.of(closedLoopTargetValue) + : null; + + inputs.goalPosition = isRunningPositionControl ? goalPosition : null; + + if (isRunningVelocityControl) { + inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue); + inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue); + } else if (isRunningPositionControl && isRunningMotionMagic) { + var targetVelocity = closedLoopReferenceSlope.getValue(); + inputs.velocityError = + RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond)); + inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity); + } else { + inputs.velocityError = null; + inputs.activeTrajectoryVelocity = null; + } } else { + // Not in closed-loop mode, set control loop values to null + inputs.positionError = null; inputs.velocityError = null; inputs.activeTrajectoryVelocity = null; + inputs.activeTrajectoryPosition = null; + inputs.goalPosition = null; } inputs.controlType = getCurrentControlType(); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c23c8bb..c0a54d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,12 +13,15 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Foot; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.KilogramSquareMeters; import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; @@ -31,7 +34,7 @@ import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.controls.RainbowAnimation; import com.ctre.phoenix6.signals.Enable5VRailValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; @@ -194,7 +197,7 @@ public class HopperConstants { public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); - public static final double GEARING = (5.0 / 1.0); + public static final double GEARING = (1.0 / 1.0); public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); @@ -227,21 +230,21 @@ public static TalonFXConfiguration getFXConfig() { config.Voltage.PeakReverseVoltage = -12.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + config.Slot0 = new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0).withKS(2.5).withKV(0.05); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -284,6 +287,8 @@ public final class ShooterConstants { public static final double IDLE_HOOD_ANGLE = 25.0; // degrees public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); + + public static final double HARD_STOP_CURRENT_LIMIT = 50.0; } public class ShooterFlywheelConstants { @@ -302,7 +307,7 @@ public class ShooterFlywheelConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0).withKS(10.0); + new Slot0Configs().withKP(18.0).withKI(0.0).withKD(0.01).withKV(0.05).withKS(8.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -542,6 +547,10 @@ public static VisionSystemSim getSystemSim() { } } + // config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(30).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0); + public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -559,19 +568,19 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + config.Slot0 = SLOT0CONFIG; config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -653,23 +662,81 @@ public static TalonFXConfiguration getFXConfig() { } public class FeederConstants { - public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0); + public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(20.0); + public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(50); + + public static String NAME = "ShooterTower"; + + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (15.0 / 30.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(5.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(12.0); + + public static TalonFXConfiguration getFXConfig(boolean invert) { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + // config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + if (invert == true) { + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } else { + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } } public class ClimberConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); public static final String MOTOR_NAME = "Climber motor"; + public static final String NAME = "Climber"; public static final Distance TOLERANCE = Inches.of(0.1); - public static final double GEARING = (5.0 / 1.0); + public static final double GEARING = 1; public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); public static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); + public static final AngularVelocity CRUISE_VELOCITY = - RadiansPerSecond.of(2 * Math.PI).times(10.0); + RadiansPerSecond.of(2 * Math.PI).times(5000.0); + public static final AngularVelocity LOWER_VELOCITY = + RadiansPerSecond.of(-2 * Math.PI).times(5000.0); + public static final AngularVelocity CALIBRATE_VELOCITY = + RadiansPerSecond.of(-2 * Math.PI).times(5000.0); + public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); public static final Distance BOTTOM = Inches.of(0.0); @@ -701,19 +768,20 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + config.Slot0 = new Slot0Configs().withKP(50).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -727,6 +795,6 @@ public static TalonFXConfiguration getFXConfig() { public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); public static final double CLIMB_SPEED = 1.0; - public static final double HARD_STOP_CURRENT_LIMIT = 50.0; + public static final double HARD_STOP_CURRENT_LIMIT = 20.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70a5332..62ca824 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,9 @@ package frc.robot; -import au.grapplerobotics.LaserCan; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.BaseStatusSignal; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,9 +29,14 @@ import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.lib.W8.mechanisms.linear.LinearMechanismReal; +import frc.lib.W8.mechanisms.linear.LinearMechanismSim; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.ClimberConstants; +import frc.robot.Constants.FeederConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; @@ -39,7 +46,7 @@ import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.BallCounter; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -67,8 +74,9 @@ public class RobotContainer { private final Hopper hopper; private final Shooter shooter; public final Intake intake; - private final BallCounter ballCounter; + // private final BallCounter ballCounter; private final Vision vision; + private final Climber climber; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -79,8 +87,10 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Check if the robot is real before using the ball counter! - if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1)); - else ballCounter = null; + // if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1)); + // else ballCounter = null; + + BaseStatusSignal.setUpdateFrequencyForAll(50.0); switch (Constants.currentMode) { case REAL: @@ -103,19 +113,17 @@ public RobotContainer() { new Shooter( new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), + "ShooterLeftFlywheel", + ShooterFlywheelConstants.getFXConfig(true), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(true), + "ShooterRightFlywheel", + ShooterFlywheelConstants.getFXConfig(false), Ports.RightFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), - Ports.TowerRoller)), + "ShooterTower", FeederConstants.getFXConfig(false), Ports.TowerRoller)), new RotaryMechanismReal( new MotorIOTalonFX( ShooterRotaryConstants.NAME, @@ -138,6 +146,14 @@ public RobotContainer() { Ports.IntakePivot), IntakePivotConstants.CONSTANTS, Optional.empty())); + climber = + new Climber( + new LinearMechanismReal( + new MotorIOTalonFX( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberMotor), + ClimberConstants.CHARACTERISTICS)); vision = new Vision( new VisionIOPhotonVision( @@ -179,7 +195,7 @@ public RobotContainer() { new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, ShooterFlywheelConstants.getFXConfig(true), - Ports.LeftFlywheel), + Ports.RightFlywheel), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, ShooterFlywheelConstants.TOLERANCE), @@ -231,6 +247,17 @@ public RobotContainer() { VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP, VisionConstants.getSystemSim())); + climber = + new Climber( + new LinearMechanismSim( + new MotorIOTalonFXSim( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberMotor), + ClimberConstants.DCMOTOR, + ClimberConstants.CARRIAGE_MASS, + ClimberConstants.CHARACTERISTICS, + false)); break; default: @@ -257,6 +284,10 @@ public RobotContainer() { new FlywheelMechanism() {}, new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); vision = new Vision(null); + + climber = + new Climber( + new LinearMechanism(ClimberConstants.NAME, ClimberConstants.CHARACTERISTICS) {}); break; } @@ -322,9 +353,9 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); - controller - .x() - .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); + // controller + // .x() + // .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); @@ -337,10 +368,46 @@ private void configureButtonBindings() { Robot.fuelSim.spawnStartingFuel(); intake.simBalls = 0; })); - controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); - controller.leftBumper().onTrue(intake.intake()); - controller.rightBumper().onTrue(intake.stowAndStopRollers()); - controller.a().onTrue(shooter.runFlywheel()); + // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); + // controller.a().onTrue(intake.intake()); + // controller.x().onTrue(intake.stowAndStopRollers()); + + // Flywheel + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(25))); + controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); + + // Intake + Spindexer + Tower + controller + .rightBumper() + .whileTrue( + Commands.parallel( + // intake.runRollers(RotationsPerSecond.of(30)), + hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(70)))); + + controller + .rightBumper() + .onFalse( + Commands.parallel( + intake.runRollers(RotationsPerSecond.of(0)), + hopper.runSpindexer(0), + shooter.runTower(RotationsPerSecond.of(0)))); + + // Intake Rollers 11 Motor: 9 Intake + controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(15)))); + controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); + + // Spindexer 1:1 + controller.x().whileTrue(hopper.runSpindexer(18)); + controller.x().onFalse(hopper.runSpindexer(0)); + + // Tower - 15 Motor:7 Tower + + controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); + controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); + + // controller.povLeft().whileTrue(climber.calibrateClimber()); + // controller.povUp().whileTrue(climber.raiseClimber()); + // controller.povDown().whileTrue(climber.lowerClimber()); } /** diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index fc20735..c891c3c 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -13,9 +13,10 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; +// // import frc.robot.subsystems.CommandSwerveDrivetrain; -// Generated by the Tuner X Swerve Project Generator +// Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -56,7 +57,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(120); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -76,17 +77,17 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 6.746031746031747; + private static final double kDriveGearRatio = 6.122448979591837; private static final double kSteerGearRatio = 21.428571428571427; private static final Distance kWheelRadius = Inches.of(2); @@ -96,8 +97,8 @@ public class TunerConstants { private static final int kPigeonId = 0; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); // Simulated voltage necessary to overcome friction private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); @@ -138,45 +139,45 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 35; private static final int kFrontLeftSteerMotorId = 12; private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.1416015625); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.06396484375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(12.25); - private static final Distance kFrontLeftYPos = Inches.of(10.25); + private static final Distance kFrontLeftXPos = Inches.of(10.75); + private static final Distance kFrontLeftYPos = Inches.of(10.75); // Front Right private static final int kFrontRightDriveMotorId = 41; private static final int kFrontRightSteerMotorId = 42; private static final int kFrontRightEncoderId = 4; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.1748046875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.27783203125); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(12.25); - private static final Distance kFrontRightYPos = Inches.of(-10.25); + private static final Distance kFrontRightXPos = Inches.of(10.75); + private static final Distance kFrontRightYPos = Inches.of(-10.75); // Back Left private static final int kBackLeftDriveMotorId = 21; private static final int kBackLeftSteerMotorId = 25; private static final int kBackLeftEncoderId = 2; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.369384765625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.380859375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-12.25); - private static final Distance kBackLeftYPos = Inches.of(10.25); + private static final Distance kBackLeftXPos = Inches.of(-10.75); + private static final Distance kBackLeftYPos = Inches.of(10.75); // Back Right private static final int kBackRightDriveMotorId = 31; private static final int kBackRightSteerMotorId = 32; private static final int kBackRightEncoderId = 3; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.388916015625); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.11279296875); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-12.25); - private static final Distance kBackRightYPos = Inches.of(-10.25); + private static final Distance kBackRightXPos = Inches.of(-10.75); + private static final Distance kBackRightYPos = Inches.of(-10.75); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> @@ -291,9 +292,9 @@ public TunerSwerveDrivetrain( * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]ᵀ, with units in meters and radians + * [x, y, theta]áµ€, with units in meters and radians * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, - * y, theta]ᵀ, with units in meters and radians + * y, theta]áµ€, with units in meters and radians * @param modules Constants for each specific module */ public TunerSwerveDrivetrain( diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4aade98..90f7371 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,19 +2,13 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.linear.LinearMechanism; import frc.robot.Constants.ClimberConstants; -import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -43,19 +37,26 @@ public boolean isAboveCurrentLimit() { } } + // public Command homeCommand() + // { + // return Commands.sequence( + // runOnce(() -> _io.runVoltage(Volts.of(-2))), + // Commands.waitUntil(() -> isAboveCurrentLimit())); + // } + @Override public void periodic() { _io.periodic(); - double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position + // double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - // The z of the Translation3D should be - // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // motor configs. - Logger.recordOutput( - "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); + // // The z of the Translation3D should be + // // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing + // // motor configs. + // Logger.recordOutput( + // "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } public Command runClimber() { @@ -69,6 +70,40 @@ public Command runClimber() { PIDSlot.SLOT_0)); } + public Command calibrateClimber() { + System.out.println(ClimberConstants.CALIBRATE_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.CALIBRATE_VELOCITY, + ClimberConstants.ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + + public Command raiseClimber() { + System.out.println(ClimberConstants.CRUISE_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.CRUISE_VELOCITY, + ClimberConstants.ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + + public Command lowerClimber() { + System.out.println(ClimberConstants.LOWER_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.LOWER_VELOCITY, ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 43cc31e..46c4b82 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -1,18 +1,21 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.units.Units.Inches; import edu.wpi.first.units.measure.*; -import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.robot.Constants.HopperConstants; +import org.littletonrobotics.junction.Logger; public class Hopper extends SubsystemBase { private FlywheelMechanism _io; + public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); + public Hopper(FlywheelMechanism io) { _io = io; } @@ -27,6 +30,21 @@ public void setGoal(double position) { PIDSlot.SLOT_0); } + // Velocity of Rollers + public void setVelocity(double velocity) { + AngularVelocity angVelo = RotationsPerSecond.of(velocity); + + _io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); + targetVelocity = angVelo; + } + + public Command runSpindexer(double velocity) { + return Commands.runOnce(() -> setVelocity(velocity), this); + } + @Override - public void periodic() {} + public void periodic() { + _io.periodic(); + Logger.recordOutput("Hopper/TargetVelocity", targetVelocity); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 414cebc..224ea29 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -27,6 +26,7 @@ public class Intake extends SubsystemBase { double pivotAngle; public double desiredAngle; public int simBalls; + public AngularVelocity targetSpeed = RotationsPerSecond.of(0); public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; @@ -36,10 +36,11 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { } // Velocity of Rollers - public void setVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); + public void setVelocity(AngularVelocity velocity) { + // AngularVelocity angVelo = RotationsPerSecond.of(velocity); - _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); + _rollerIO.runVelocity(velocity, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); + targetSpeed = velocity; } public Command setPivotAngle(Angle pivotAngle) { @@ -63,12 +64,17 @@ public Angle getPosition() { } public void stop() { - setVelocity(0); + setVelocity(RotationsPerSecond.of(0)); + } + + public Command runRollers(AngularVelocity velocity) { + return Commands.run(() -> setVelocity(velocity), this); } public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), + Commands.run( + () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); } @@ -85,7 +91,8 @@ public boolean canIntake() { public Command stowAndStopRollers() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), + Commands.run( + () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), setStowAngle(IntakePivotConstants.STOW_ANGLE)); } @@ -102,10 +109,13 @@ private Command setStowAngle(Angle stowAngle) { @Override public void periodic() { - if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) - _pivotIO.runVoltage(Volts.of(0.25)); + // if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) + // _pivotIO.runVoltage(Volts.of(0.25)); + _rollerIO.periodic(); + Logger.recordOutput("Intake/TargetSpeed", targetSpeed); _pivotIO.periodic(); + // Logger.recordOutput("Intake/TargetPivot", null); Logger.recordOutput( "3DField/1_Intake", new Pose3d( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2cc0c7a..60b5ddf 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,9 +1,9 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -13,7 +13,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -37,6 +36,9 @@ public class Shooter extends SubsystemBase { private double desiredVelo; private double hoodAngle; + public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); + public AngularVelocity feederTargetVelocity = RotationsPerSecond.of(0.0); + public Shooter( FlywheelMechanism lflywheel, FlywheelMechanism rflywheel, @@ -49,19 +51,20 @@ public Shooter( } // Sets feeder motor speed - public void runFeeder() { - _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + public void runFeeder(AngularVelocity velocity) { + _feeder.runVelocity(velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_0); + feederTargetVelocity = velocity; } // Sets the flywheel velocity based on an input. - public void setFlywheelVelocity(double velocity) { + public void setFlywheelVelocity(AngularVelocity velocity) { // store the desired velocity then send converted velocity to the mechanism - this.desiredVelo = velocity; - AngularVelocity angVelo = RotationsPerSecond.of(velocity); - AngularVelocity negangVelo = RotationsPerSecond.of(velocity); - _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + // this.desiredVelo = velocity; + // AngularVelocity angVelo = RotationsPerSecond.of(velocity); + // AngularVelocity negangVelo = RotationsPerSecond.of(velocity); + _lflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _rflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + targetVelocity = velocity; } public enum State { @@ -125,7 +128,25 @@ public boolean hoodAtAngle() { return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; } - public Command shoot(double velocity) { + public boolean isAboveCurrentLimit() { + if (_hood.getSupplyCurrent().in(Amps) > ShooterConstants.HARD_STOP_CURRENT_LIMIT) { + return true; + } else { + return false; + } + } + + public Command calibrateHood() { + return this.run( + () -> + _hood.runVelocity( + ShooterConstants.HOOD_VELOCITY, + ShooterConstants.HOOD_ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + + public Command shoot(AngularVelocity velocity) { // Prepare targets return Commands.sequence( // Set and wait in parallel for both hood and flywheel @@ -133,13 +154,20 @@ public Command shoot(double velocity) { Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity), Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), // feed once ready - Commands.runOnce(() -> runFeeder()), + Commands.runOnce(() -> runFeeder(FeederConstants.FEED_SPEED)), // stop flywheel when finished - Commands.runOnce(() -> setFlywheelVelocity(0))); + Commands.runOnce(() -> setFlywheelVelocity(RotationsPerSecond.of(0.0)))); + } + + public Command runFlywheel(AngularVelocity velocity) { + System.out.println("Flywheel"); + + return Commands.run(() -> setFlywheelVelocity(velocity), this); } - public Command runFlywheel() { - return Commands.runOnce(() -> setFlywheelVelocity(1)); + public Command runTower(AngularVelocity velocity) { + System.out.println("Tower"); + return Commands.run(() -> runFeeder(velocity), this); } public void simShoot() { @@ -180,19 +208,24 @@ public void simShoot() { public void periodic() { _hood.periodic(); + _lflywheel.periodic(); + _rflywheel.periodic(); + _feeder.periodic(); + Logger.recordOutput("Flywheel/TargetVelocity", targetVelocity); + Logger.recordOutput("Feeder/TargetVelocity", feederTargetVelocity); // _feeder.periodic(); // _flywheel.periodic(); - double pitch = - Math.toRadians( - Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position + // double pitch = + // Math.toRadians( + // Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing // motor configs. - Logger.recordOutput( - "3DField/3_Hood", - new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); + // Logger.recordOutput( + // "3DField/3_Hood", + // new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); - _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index bb6a6d6..39878e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -172,28 +172,38 @@ public ModuleIOTalonFX( turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames + // Configure periodic frames with optimized frequencies + // Odometry signals need high frequency for accurate pose estimation BaseStatusSignal.setUpdateFrequencyForAll( Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + + // Control signals need reduced frequency + BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, turnVelocity); + + // Telemetry signals can use very low frequency BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + 10.0, driveAppliedVolts, driveCurrent, turnAppliedVolts, turnCurrent); + + // Absolute encoder only needs very low updates since it's mainly used for initialization + BaseStatusSignal.setUpdateFrequencyForAll(20.0, turnAbsolutePosition); + + // Optimize bus utilization for each motor with longer timeout for better optimization ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + cancoder.optimizeBusUtilization(1.0); } @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh all signals - var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + // Refresh signals in priority groups + // Refresh critical odometry signals first + var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity); + var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity); + + // Refresh telemetry signals + BaseStatusSignal.refreshAll(driveAppliedVolts, driveCurrent); + BaseStatusSignal.refreshAll(turnAppliedVolts, turnCurrent); + + // Refresh absolute encoder with lower priority var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); // Update drive inputs diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 162ad66..868ae9d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "26.0.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "26.0.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "26.0.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e593..e8196c3 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.4", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0a948bd..62f135c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-2" + "version": "v2026.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-2" + "version": "v2026.3.1" } ] } \ No newline at end of file