From 52163858d83ef22b98725157afb3a61d15669c83 Mon Sep 17 00:00:00 2001 From: DanPeled Date: Fri, 29 May 2026 12:25:18 +0300 Subject: [PATCH 1/3] Improved Turret shoot cmd, correct turret pose calc --- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 28 ++-- .../IntakeSubsystem/IntakeConstants.java | 4 +- .../subsystems/indexer/IndexerConstants.java | 6 +- .../subsystems/indexer/IndexerSubsystem.java | 2 +- .../subsystems/shooter/HoodSubsystem.java | 34 ++-- .../subsystems/shooter/ShootToTargetCmd.java | 151 ++++++++++++------ .../subsystems/shooter/ShooterConstants.java | 19 ++- .../subsystems/shooter/ShooterSubsystem.java | 79 +++++---- .../subsystems/shooter/TurretSubsystem.java | 7 +- .../subsystems/swerve/SwerveSubsystem.java | 7 + .../vision/TurretCameraVisionIO.java | 1 - .../subsystems/vision/VisionConstants.java | 4 +- 13 files changed, 216 insertions(+), 130 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e76bb4..c4c59bf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,7 +19,7 @@ public class Robot extends LoggedRobot { private final CommandScheduler m_scheduler = CommandScheduler.getInstance(); private static Robot instance; private Command m_autonomousCommand; - private ShooterContainer m_robotContainer; + private RobotContainer m_robotContainer; private Timer disabledTimer; private final Timer m_alertTimer = new Timer(); private final Alert m_canErrorAlert = @@ -60,7 +60,7 @@ public static Robot getInstance() { @Override public void robotInit() { - m_robotContainer = new ShooterContainer(); + m_robotContainer = new RobotContainer(); disabledTimer = new Timer(); if (isSimulation()) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 445f05e..f62d58d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,14 +20,19 @@ import robottester.RobotTester; public class RobotContainer { - private boolean m_testsAdded = false; // should only add tests once - private final IndexerSubsystem m_indexing = new IndexerSubsystem(); private final CommandXboxController m_driverController = new CommandXboxController(0); + private final IndexerSubsystem m_indexing = new IndexerSubsystem(); private final SwerveSubsystem m_swerve = new SwerveSubsystem(SwerveConstants.SwerveConfig.OCTANE); private final IntakeSubsystem m_intake = new IntakeSubsystem(); private final ShooterSubsystem m_shooter = - new ShooterSubsystem() + new ShooterSubsystem( + new Trigger( + () -> { + return m_intake.getArmSetpoint().lt(Degrees.of(30)) + && m_intake.getArmInputs().position.lt(Degrees.of(16)); + }), + m_driverController.rightTrigger(0.5)) .withRobotEstimatedPose(m_swerve::getPose) .withRobotFieldVelocity(m_swerve::getFieldVelocity) .withRobotRelativeVelocity(m_swerve::getRobotVelocity); @@ -40,7 +45,8 @@ public class RobotContainer { new TurretCameraVisionIO("limelight-turret", VecBuilder.fill(0.3, 0.3, 9999999)) .withTurretRotationSupplier( () -> - new Rotation3d(Degrees.of(0), Degrees.of(0), m_shooter.getTurretAngle()))) + new Rotation3d( + Degrees.of(0), Degrees.of(0), m_shooter.getTurretAngle().times(-1)))) .withSpeedsSupplier(m_swerve::getFieldVelocity); private final RobotTester robotTester = new RobotTester(); @@ -59,18 +65,14 @@ public RobotContainer() { } private void configureBindings() { + m_driverController.leftTrigger(0.5).whileTrue(m_indexing.throwUp()); m_driverController.leftBumper().toggleOnTrue(intake()); m_driverController.rightBumper().onTrue(halfIntake()); - new Trigger( - () -> { - return m_intake.getArmSetpoint().lt(Degrees.of(45)) - && m_intake.getArmInputs().position.lt(Degrees.of(16)); - }) - .onTrue(m_shooter.closeHood()); - - m_driverController.rightTrigger(0.5).whileTrue(m_shooter.trackHub(m_indexing)); + m_driverController.rightTrigger(0.5).onTrue(m_shooter.trackHub(m_indexing)); m_driverController.a().onTrue(m_shooter.closeHood()); + m_driverController.x().onTrue(m_swerve.lockPose()); + m_driverController.y().onTrue(m_intake.closeArm()); m_swerve.configureBindings(m_driverController); m_intake.configureBindings(m_driverController); @@ -81,7 +83,7 @@ private void configureBindings() { private void configureTests() { m_swerve.configureTests(robotTester); m_intake.configureTests(robotTester); - // m_shooter.configureTests(robotTester); + m_shooter.configureTests(robotTester); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem/IntakeConstants.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem/IntakeConstants.java index a06369a..89a6953 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem/IntakeConstants.java @@ -26,8 +26,8 @@ public final class Arm { public static final Current kStatorCurrentLimit = Amps.of(60); public static final Angle kClosedPosition = Degrees.of(6); - public static final Angle kOpenPosition = Degrees.of(89); - public static final Angle kHalfOpenPosition = Degrees.of(20); + public static final Angle kOpenPosition = Degrees.of(91); + public static final Angle kHalfOpenPosition = Degrees.of(14); public static final MechanismGearing kGearing = new MechanismGearing(GearBox.fromStages("5:1", "5:1", "5:1", "41:16")); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 6a3bd5d..86c2bba 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -7,7 +7,7 @@ import yams.gearing.MechanismGearing; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; -public class IndexerConstants { // TODO: set real values for these constants +public class IndexerConstants { public static class Transporter { public static final int kMotorID = 40; public static final double kTransportingMotorVoltage = -0.9; @@ -21,9 +21,9 @@ public static class Transporter { public static class Feeder { public static final int kMotorID = 41; public static final double kFeedingMotorVoltage = 1; - public static final double kReversingMotorVoltage = -0.1; + public static final double kReversingMotorVoltage = -0.8; public static final double kStuckMotorVoltage = -1; - public static final boolean kIsInverted = true; + public static final boolean kIsInverted = false; public static final Current kStatorCurrentLimit = Amps.of(40); public static final MotorMode kMotorMode = MotorMode.COAST; public static final MechanismGearing kGearing = diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 714c719..4611ef3 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -30,7 +30,7 @@ public double getFeederDutyCycle() { } public Command throwUp() { - return m_feeder.reverse(); + return m_feeder.reverse().alongWith(m_transporter.setDutyCycle(() -> -0.5)); } public Command feederReverse() { diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/HoodSubsystem.java index bb0a82f..ade3158 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodSubsystem.java @@ -25,11 +25,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.shooter.ShooterConstants.Hood.FeedForward; import frc.robot.subsystems.shooter.ShooterConstants.Hood.PID; import frc.robot.subsystems.shooter.ShooterConstants.Hood.PhysicalPropreties; +import java.util.Optional; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; +import org.msgpack.core.annotations.Nullable; import robottester.RobotTester; import robottester.TestOption; import yams.math.ExponentialProfilePIDController; @@ -54,13 +57,15 @@ public static class HoodInputs { public double angleEncoder = 0.0; } + private Optional m_canBeOpen = Optional.empty(); + // private final Debouncer m_resetEncoderDebouncer = new Debouncer(0.1); private final SparkMax m_hoodMotor = new SparkMax(kMotorID, MotorType.kBrushless); // private final Encoder m_hoodEncoder = - // new Encoder( - // ShooterConstants.Hood.kEncoderIDA, - // ShooterConstants.Hood.kEncoderIDB, - // ShooterConstants.Hood.kIsEncoderInverted); + // new Encoder( + // ShooterConstants.Hood.kEncoderIDA, + // ShooterConstants.Hood.kEncoderIDB, + // ShooterConstants.Hood.kIsEncoderInverted); private final Timer m_resetEncoderTimer = new Timer(); private final ExponentialProfilePIDController m_pid = @@ -80,8 +85,8 @@ public static class HoodInputs { private final HoodInputsAutoLogged m_inputs = new HoodInputsAutoLogged(); private boolean m_isHomed = false; - public HoodSubsystem() { - + public HoodSubsystem(@Nullable Trigger canBeOpenTrigger) { + m_canBeOpen = Optional.ofNullable(canBeOpenTrigger); m_hoodMotorConfig = new SmartMotorControllerConfig(this) .withClosedLoopController(m_pid) @@ -148,7 +153,11 @@ public Command runAngle(Angle angle) { } public void runAngleDirect(Angle angle) { - m_hoodSMC.setPosition(angle); + if (m_canBeOpen.isPresent() && m_canBeOpen.get().getAsBoolean()) { + m_hoodSMC.setPosition(Degrees.of(5)); + } else { + m_hoodSMC.setPosition(angle); + } } public Angle getAngle() { @@ -170,10 +179,11 @@ public void periodic() { // m_hoodSMC.setEncoderPosition(Degrees.of(getEncoderAngle())); // if (m_resetEncoderDebouncer.calculate( - // m_hoodSMC.getVoltage().lte(Volts.of(0)) && m_hoodSMC.getStatorCurrent().gte(Amps.of(5)))) + // m_hoodSMC.getVoltage().lte(Volts.of(0)) && + // m_hoodSMC.getStatorCurrent().gte(Amps.of(5)))) // { - // m_hoodSMC.setVoltage(Volts.of(0)); - // m_hoodSMC.setEncoderPosition(Degrees.of(0)); + // m_hoodSMC.setVoltage(Volts.of(0)); + // m_hoodSMC.setEncoderPosition(Degrees.of(0)); // } } @@ -210,4 +220,8 @@ private Command homing() { }) .andThen(m_hood.runTo(Degrees.of(2), Degrees.of(1))); } + + public boolean isAtSetpoint() { + return m_hood.isNear(Degrees.of(m_inputs.setpoint), Degrees.of(1.2)).getAsBoolean(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java b/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java index da0110a..2b97283 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java @@ -1,104 +1,119 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.shooter.kinematics.LaunchCalculator; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class ShootToTargetCmd extends Command { + private static final double kStartupReverseTime = 0.5; + private static final double kFeederCurrentThreshold = 38.0; + + private enum FeedState { + kStartupReverse, + kWaitingForEntry, + kFeeding + } + private final IndexerSubsystem m_indexer; private final TurretSubsystem m_turret; private final HoodSubsystem m_hood; private final FlywheelSubsystem m_flywheel; - private Supplier estimatedRobotPoseSupplier; - private Supplier robotSpeedsSupplier; - private Supplier fieldSpeedsSupplier; + private Supplier m_robotPoseSupplier = Pose2d::new; + private Supplier m_robotRelativeSpeedsSupplier = ChassisSpeeds::new; + private Supplier m_fieldRelativeSpeedsSupplier = ChassisSpeeds::new; + private Supplier m_targetSupplier = Translation2d::new; + + private final Timer m_stateTimer = new Timer(); + private final Debouncer m_feederCurrentDebouncer = new Debouncer(0.4); + private final Trigger m_runFeederTrigger; - private boolean m_isOnStartupReverse = true; - private Timer m_startupTimer = new Timer(); + private FeedState m_feedState = FeedState.kStartupReverse; + + public ShootToTargetCmd( + TurretSubsystem turret, + HoodSubsystem hood, + FlywheelSubsystem flywheel, + IndexerSubsystem indexer, + Trigger runFeeder) { + m_indexer = indexer; + m_turret = turret; + m_hood = hood; + m_flywheel = flywheel; + m_runFeederTrigger = runFeeder; - private Supplier m_target = () -> new Translation2d(); + addRequirements(turret, hood, flywheel); - public ShootToTargetCmd withRobotFieldVelocity(Supplier fieldSpeeds) { LaunchCalculator.getInstance().setRobotToTurret(ShooterConstants.kRobotToTurret2DTransform); - fieldSpeedsSupplier = fieldSpeeds; + } + + public ShootToTargetCmd withRobotFieldVelocity(Supplier fieldSpeedsSupplier) { + + m_fieldRelativeSpeedsSupplier = fieldSpeedsSupplier; return this; } - public ShootToTargetCmd withRobotRelativeVelocity(Supplier robotSpeeds) { - robotSpeedsSupplier = robotSpeeds; + public ShootToTargetCmd withRobotRelativeVelocity(Supplier robotSpeedsSupplier) { + + m_robotRelativeSpeedsSupplier = robotSpeedsSupplier; return this; } - public ShootToTargetCmd withRobotEstimatedPose(Supplier robotPose) { - estimatedRobotPoseSupplier = robotPose; + public ShootToTargetCmd withRobotEstimatedPose(Supplier robotPoseSupplier) { + + m_robotPoseSupplier = robotPoseSupplier; return this; } public ShootToTargetCmd withTarget(Translation2d target) { - m_target = () -> target; - return this; + return withTarget(() -> target); } public ShootToTargetCmd withTarget(Supplier targetSupplier) { - m_target = targetSupplier; - return this; - } - public ShootToTargetCmd( - TurretSubsystem turret, - HoodSubsystem hood, - FlywheelSubsystem flywheel, - IndexerSubsystem indexer) { - m_indexer = indexer; - m_turret = turret; - m_hood = hood; - m_flywheel = flywheel; - addRequirements(m_turret, m_hood, m_flywheel); + m_targetSupplier = targetSupplier; + return this; } @Override public void initialize() { - m_startupTimer.restart(); + setFeedState(FeedState.kStartupReverse); } @Override public void execute() { - if (m_isOnStartupReverse) { - m_indexer.reverseDirect(); - if (m_startupTimer.get() >= 0.5) { - m_isOnStartupReverse = false; - m_startupTimer.reset(); - } - } else if (m_flywheel.isReadyToShoot()) { - if (m_startupTimer.get() >= 5) { - m_isOnStartupReverse = true; - m_startupTimer.reset(); - } - if (m_indexer.getFeederCurrent() > 38) { - m_isOnStartupReverse = true; - m_startupTimer.restart(); - } - m_indexer.feedDirect(); - } else { - m_indexer.reverseDirect(); + if (!m_runFeederTrigger.getAsBoolean() && m_feedState != FeedState.kStartupReverse) { + setFeedState(FeedState.kWaitingForEntry); } + updateShooter(); + + switch (m_feedState) { + case kStartupReverse -> handleStartupReverse(); + case kWaitingForEntry -> handleWaitingForReady(); + case kFeeding -> handleFeeding(); + } + } + + private void updateShooter() { + Translation2d target = m_targetSupplier.get(); - Translation2d target = m_target.get(); Logger.recordOutput("LaunchCalculator/Target", new Pose2d(target, Rotation2d.kZero)); + LaunchCalculator.ShotParams params = LaunchCalculator.getInstance() .calculate( - estimatedRobotPoseSupplier.get(), - robotSpeedsSupplier.get(), - fieldSpeedsSupplier.get(), + m_robotPoseSupplier.get(), + m_robotRelativeSpeedsSupplier.get(), + m_fieldRelativeSpeedsSupplier.get(), target); m_turret.runAngleDirect(params.turretAngle()); @@ -106,6 +121,42 @@ public void execute() { m_hood.runAngleDirect(params.hoodAngle()); } + private void handleStartupReverse() { + m_indexer.reverseDirect(); + + if (m_stateTimer.hasElapsed(kStartupReverseTime)) { + setFeedState(FeedState.kWaitingForEntry); + } + } + + private void handleWaitingForReady() { + m_indexer.reverseDirect(); + + if (isShooterReady() && m_runFeederTrigger.getAsBoolean()) { + setFeedState(FeedState.kFeeding); + } + } + + private void handleFeeding() { + m_indexer.feedDirect(); + + boolean feederStalled = + m_feederCurrentDebouncer.calculate(m_indexer.getFeederCurrent() > kFeederCurrentThreshold); + + if (feederStalled) { + setFeedState(FeedState.kStartupReverse); + } + } + + private boolean isShooterReady() { + return m_flywheel.isReadyToShoot() && m_turret.isAtSetpoint() && m_hood.isAtSetpoint(); + } + + private void setFeedState(FeedState newState) { + m_feedState = newState; + m_stateTimer.restart(); + } + @Override public void end(boolean interrupted) { m_indexer.stop(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 10016fe..7ae1ede 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -29,18 +29,18 @@ public class ShooterConstants { public static final Transform3d kRobotToTurretTransform = new Transform3d( - new Translation3d(Millimeters.of(127.147), Millimeters.of(0), Millimeters.of(363.604)), + new Translation3d(Millimeters.of(145.55), Millimeters.of(0), Millimeters.of(266.71)), new Rotation3d()); public static final Transform2d kRobotToTurret2DTransform = new Transform2d( - new Translation2d(Millimeters.of(127.147), Millimeters.of(0)), new Rotation2d()); + new Translation2d( + kRobotToTurretTransform.getMeasureX(), kRobotToTurretTransform.getMeasureY()), + new Rotation2d()); public static final Transform3d kTurretToCameraTransform = new Transform3d( - new Translation3d( - // Millimeters.of(31.16056), Millimeters.of(139.926395), Millimeters.of(149.197)), - Millimeters.of(52.412207), Millimeters.of(140.0), Millimeters.of(125.053649)), + new Translation3d(Millimeters.of(75.94), Millimeters.of(118.15), Millimeters.of(134.86)), new Rotation3d(Degrees.of(0), Degrees.of(10), Degrees.of(0))); public static class Turret { @@ -57,16 +57,15 @@ public static class PhysicalProperties { new MechanismGearing(GearBox.fromStages("5:1", "3:1", "130:24")); public static final Angle kDefaultAngle = Degrees.of(0); - public static final Angle kSoftMaxAngle = Degrees.of(0); - public static final Angle kSoftMinAngle = Degrees.of(4); + public static final Angle kSoftMaxAngle = Degrees.of(359.8); + public static final Angle kSoftMinAngle = Degrees.of(0.1); public static final Angle kHardMaxAngle = Degrees.of(360); - public static final Angle kHardMinAngle = Degrees.of(360); + public static final Angle kHardMinAngle = Degrees.of(0); public static final Angle kSafeAngleForBoxToClose = Degrees.of(180); } public static class PID { - - public static final AngularVelocity kMaxVelocity = DegreesPerSecond.of(680); + public static final AngularVelocity kMaxVelocity = DegreesPerSecond.of(9000); public static final AngularAcceleration kMaxAcceleration = DegreesPerSecondPerSecond.of(1020); public static double kP = 45; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 2a339be..57f135b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.FieldConstants; import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.shooter.kinematics.AngleFlag; @@ -20,9 +21,10 @@ import robottester.RobotTester; public class ShooterSubsystem { - private final HoodSubsystem m_hood = new HoodSubsystem(); - private final FlywheelSubsystem m_flywheel = new FlywheelSubsystem(); - private final TurretSubsystem m_turret = new TurretSubsystem(); + private final HoodSubsystem m_hood; + private final FlywheelSubsystem m_flywheel; + private final TurretSubsystem m_turret; + private static final LaunchCalculator s_launchCalculator = LaunchCalculator.getInstance() .withTurretAngleFormat(new AngleFormat(AngleFlag.kCWPositive, AngleFlag.kRange0To360)); @@ -31,24 +33,46 @@ public class ShooterSubsystem { private Supplier m_fieldRelativeSpeeds; private Supplier m_robotRelativeSpeeds; + private final Translation2d m_hubTarget = + AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()); + + private final Translation2d m_leftFeedTarget = + AllianceFlipUtil.apply(FieldConstants.LeftBump.nearLeftCorner); + + private final Translation2d m_rightFeedTarget = + AllianceFlipUtil.apply(FieldConstants.RightBump.nearRightCorner); + + private final Alliance m_alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + + private final double m_hubX = AllianceFlipUtil.applyX(FieldConstants.Hub.topCenterPoint.getX()); + private final Trigger m_runReverseFeed; + + public ShooterSubsystem(Trigger canHoodBeOpen, Trigger runReverseFeed) { + m_hood = new HoodSubsystem(canHoodBeOpen); + m_flywheel = new FlywheelSubsystem(); + m_turret = new TurretSubsystem(); + m_runReverseFeed = runReverseFeed; + } + public ShooterSubsystem withRobotFieldVelocity(Supplier fieldSpeeds) { + m_fieldRelativeSpeeds = fieldSpeeds; return this; } public ShooterSubsystem withRobotRelativeVelocity(Supplier robotSpeeds) { + m_robotRelativeSpeeds = robotSpeeds; return this; } public ShooterSubsystem withRobotEstimatedPose(Supplier robotPose) { + m_robotPose = robotPose; return this; } - public void configureBindings(CommandXboxController controller) { - // Shoo - } + public void configureBindings(CommandXboxController controller) {} public void configureTests(RobotTester tester) { m_hood.configureTests(tester); @@ -57,43 +81,34 @@ public void configureTests(RobotTester tester) { } public Command shootToAutoAimedTarget(IndexerSubsystem indexer) { - return new ShootToTargetCmd(m_turret, m_hood, m_flywheel, indexer) - .withTarget( - () -> - shouldAimToHub() - ? AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()) - : getFeedingTarget()) + return new ShootToTargetCmd(m_turret, m_hood, m_flywheel, indexer, m_runReverseFeed) + .withTarget(this::getCurrentTarget) .withRobotEstimatedPose(m_robotPose) .withRobotFieldVelocity(m_fieldRelativeSpeeds) .withRobotRelativeVelocity(m_robotRelativeSpeeds); } - private Translation2d getFeedingTarget() { - Pose2d robotPose = m_robotPose.get(); + private Translation2d getCurrentTarget() { + Pose2d pose = m_robotPose.get(); + + if (shouldAimToHub(pose)) { + return m_hubTarget; + } + + return getFeedingTarget(pose); + } - boolean shouldChooseRightCorner = - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + private Translation2d getFeedingTarget(Pose2d robotPose) { + boolean chooseRightCorner = + m_alliance == Alliance.Blue ? robotPose.getY() < FieldConstants.Hub.topCenterPoint.getY() : robotPose.getY() > FieldConstants.Hub.topCenterPoint.getY(); - return AllianceFlipUtil.apply( - shouldChooseRightCorner - ? FieldConstants.RightBump.nearRightCorner - : FieldConstants.LeftBump.nearLeftCorner); + return chooseRightCorner ? m_rightFeedTarget : m_leftFeedTarget; } - private boolean shouldAimToHub() { - double x = m_robotPose.get().getX(); - double hubX = AllianceFlipUtil.applyX(FieldConstants.Hub.topCenterPoint.getX()); - switch (DriverStation.getAlliance().orElse(Alliance.Blue)) { - case Blue -> { - return x <= hubX; - } - case Red -> { - return x >= hubX; - } - } - return true; + private boolean shouldAimToHub(Pose2d robotPose) { + return m_alliance == Alliance.Blue ? robotPose.getX() <= m_hubX : robotPose.getX() >= m_hubX; } public Command trackHub(IndexerSubsystem indexer) { diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 2febfa1..1d4e7c7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -39,7 +39,6 @@ import yams.motorcontrollers.local.SparkWrapper; public class TurretSubsystem extends SubsystemBase { - public TurretSubsystem() { m_turretSMC.setEncoderPosition(Degrees.of(0)); SmartDashboard.putData( @@ -83,7 +82,7 @@ public static class TurretInputs { .withClosedLoopController(pid) .withGearing(PhysicalProperties.kGearing) .withIdleMode(MotorMode.COAST) - .withTelemetry("TurretMotor", TelemetryVerbosity.MID) + .withTelemetry("TurretMotor", TelemetryVerbosity.HIGH) .withStatorCurrentLimit(ShooterConstants.Turret.kStatorCurrentLimit) .withMotorInverted(ShooterConstants.Turret.kIsInverted) .withClosedLoopRampRate(Seconds.of(0.25)) @@ -99,7 +98,7 @@ public static class TurretInputs { .withTelemetry("TurretMech", TelemetryVerbosity.HIGH) .withStartingPosition(PhysicalProperties.kSoftMinAngle) .withMOI(PhysicalProperties.kDiamater, PhysicalProperties.kMass) - // .withSoftLimits(PhysicalProperties.kSoftMinAngle, PhysicalProperties.kSoftMaxAngle) + .withSoftLimits(PhysicalProperties.kSoftMinAngle, PhysicalProperties.kSoftMaxAngle) .withHardLimit(PhysicalProperties.kHardMinAngle, PhysicalProperties.kHardMaxAngle); // .withWrapping(PhysicalProperties.kSoftMinAngle, PhysicalProperties.kSoftMaxAngle); @@ -218,6 +217,6 @@ public Command doSomething(Command action) { } public boolean isAtSetpoint() { - return m_inputs.setpoint.isNear(m_inputs.position, Degrees.of(2)); + return m_inputs.setpoint.isNear(m_inputs.position, Degrees.of(4)); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 951dbae..c75aff1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -329,6 +329,13 @@ public void lock() { swerveDrive.lockPose(); } + public Command lockPose() { + return run( + () -> { + lock(); + }); + } + /** * Gets the current pitch angle of the robot, as reported by the imu. * diff --git a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java index 01874a1..32ada1b 100644 --- a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java @@ -18,7 +18,6 @@ import org.littletonrobotics.junction.Logger; public class TurretCameraVisionIO implements VisionIO { - private Supplier m_turretRotationSupplier = () -> new Rotation3d(); private Limelight m_limelight; private LimelightPoseEstimator limelightPoseEstimator; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index e377940..eb2b299 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -9,7 +9,7 @@ public static class kSynapse { public static final String kCoprocessorName = "Synapse 2026"; public enum Cameras { - kMonochrome("TurretCam", VecBuilder.fill(0.3, 0.3, 9999)); // TODO : get stdDevs + kMonochrome("TurretCam", VecBuilder.fill(0.25, 0.25, 9999)); // TODO : get stdDevs public final String cameraName; public final Vector stdDevs; @@ -25,6 +25,6 @@ public static class kLimelight { public static final String kLimelightName = "limelight-gabriel"; public static final Vector stdDevs = - VecBuilder.fill(0.3, 0.3, 9999999); // TODO: get stdDevs + VecBuilder.fill(9999999, 9999999, 9999999); // TODO: get stdDevs } } From b93b8f15b26c1dd1e643af1e159dac9a6b4ca9be Mon Sep 17 00:00:00 2001 From: DanPeled Date: Fri, 29 May 2026 14:57:31 +0300 Subject: [PATCH 2/3] added stdv scaling --- .../subsystems/swerve/SwerveSubsystem.java | 96 ++++++++++--------- .../vision/TurretCameraVisionIO.java | 68 +++++++------ .../subsystems/vision/VisionIOLimelight.java | 33 +++++-- .../subsystems/vision/VisionSubsystem.java | 20 ++-- 4 files changed, 125 insertions(+), 92 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index c75aff1..4353291 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -3,9 +3,10 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meter; -import static edu.wpi.first.units.Units.Seconds; +import java.io.File; +import java.io.IOException; +import java.util.Arrays; +import java.util.function.Supplier; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -16,6 +17,9 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; @@ -24,10 +28,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; -import java.io.File; -import java.io.IOException; -import java.util.Arrays; -import java.util.function.Supplier; import limelight.networktables.AngularVelocity3d; import limelight.networktables.Orientation3d; import robottester.RobotTester; @@ -56,17 +56,16 @@ public SwerveSubsystem(SwerveConstants.SwerveConfig config) { */ public SwerveSubsystem(File directory) { boolean blueAlliance = false; - Pose2d startingPose = - blueAlliance - ? new Pose2d(new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0)) - : new Pose2d(new Translation2d(Meter.of(16), Meter.of(4)), Rotation2d.fromDegrees(180)); + Pose2d startingPose = blueAlliance + ? new Pose2d(new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0)) + : new Pose2d(new Translation2d(Meter.of(16), Meter.of(4)), Rotation2d.fromDegrees(180)); - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being + // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary + // objects being // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = - new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose); + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose); } catch (IOException e) { throw new RuntimeException(e); } @@ -80,20 +79,20 @@ public SwerveSubsystem(File directory) { swerveDrive.setModuleEncoderAutoSynchronize( false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders // periodically when they are not moving. - // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal + // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used + // over the internal // encoder and push the offsets onto it. Throws warning if not possible } public void configureBindings(CommandXboxController driverXbox) { - SwerveInputStream driveAngularVelocity = - SwerveInputStream.of( - getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(driverXbox::getRightX) - .deadband(Constants.OperatorConstants.DEADBAND) - .scaleTranslation(0.9) - .allianceRelativeControl(true); + SwerveInputStream driveAngularVelocity = SwerveInputStream.of( + getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(driverXbox::getRightX) + .deadband(Constants.OperatorConstants.DEADBAND) + .scaleTranslation(0.9) + .allianceRelativeControl(true); Command driveFieldOrientedAnglularVelocity = driveFieldOriented(driveAngularVelocity); setDefaultCommand(driveFieldOrientedAnglularVelocity); @@ -104,13 +103,13 @@ public void configureBindings(CommandXboxController driverXbox) { public void configureTests(RobotTester tester) { tester.addTestOption( TestOption.fromCommand( - Commands.sequence( - Commands.runOnce( - () -> { - setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - }), - driveFieldOriented(() -> new ChassisSpeeds(1, 1, 0)) - .until(() -> getFieldVelocity().vyMetersPerSecond > 0.5))) + Commands.sequence( + Commands.runOnce( + () -> { + setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + }), + driveFieldOriented(() -> new ChassisSpeeds(1, 1, 0)) + .until(() -> getFieldVelocity().vyMetersPerSecond > 0.5))) .withTestTimeout(Seconds.of(2)) .withCleanup( () -> { @@ -125,10 +124,12 @@ public void configureTests(RobotTester tester) { } @Override - public void periodic() {} + public void periodic() { + } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + } /** * Command to characterize the robot drive motors using SysId @@ -208,8 +209,10 @@ public SwerveDriveKinematics getKinematics() { } /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when - * calling this method. However, if either gyro angle or module position is reset, this must be + * Resets odometry to the given pose. Gyro angle and module positions do not + * need to be reset when + * calling this method. However, if either gyro angle or module position is + * reset, this must be * called in order for odometry to keep working. * * @param initialHolonomicPose The pose to set the odometry to @@ -219,7 +222,8 @@ public void resetOdometry(Pose2d initialHolonomicPose) { } /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * Gets the current pose (position and rotation) of the robot, as reported by + * odometry. * * @return The robot's pose */ @@ -246,7 +250,8 @@ public void postTrajectory(Trajectory trajectory) { } /** - * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + * Resets the gyro angle to zero and resets odometry to the same position, but + * facing toward 0. */ public void zeroGyro() { swerveDrive.zeroGyro(); @@ -255,7 +260,8 @@ public void zeroGyro() { /** * Checks if the alliance is red, defaults to false if alliance isn't available. * - * @return true if the red alliance, false if blue. Defaults to false if none is available. + * @return true if the red alliance, false if blue. Defaults to false if none is + * available. */ private boolean isRedAlliance() { var alliance = DriverStation.getAlliance(); @@ -263,9 +269,11 @@ private boolean isRedAlliance() { } /** - * This will zero (calibrate) the robot to assume the current position is facing forward + * This will zero (calibrate) the robot to assume the current position is facing + * forward * - *

If red alliance rotate the robot 180 after the drviebase zero command + *

+ * If red alliance rotate the robot 180 after the drviebase zero command */ public void zeroGyroWithAlliance() { if (isRedAlliance()) { @@ -287,8 +295,10 @@ public void setMotorBrake(boolean brake) { } /** - * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the - * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls + * Gets the current yaw angle of the robot, as reported by the swerve pose + * estimator in the + * underlying drivebase. Note, this is not the raw gyro reading, this may be + * corrected from calls * to resetOdometry(). * * @return The yaw angle @@ -330,7 +340,7 @@ public void lock() { } public Command lockPose() { - return run( + return runOnce( () -> { lock(); }); diff --git a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java index 32ada1b..b1a00b2 100644 --- a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java @@ -1,5 +1,9 @@ package frc.robot.subsystems.vision; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -9,13 +13,11 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.shooter.ShooterConstants; -import java.util.function.Supplier; import limelight.Limelight; import limelight.networktables.LimelightData; import limelight.networktables.LimelightPipelineData; import limelight.networktables.LimelightPoseEstimator; import limelight.networktables.LimelightPoseEstimator.EstimationMode; -import org.littletonrobotics.junction.Logger; public class TurretCameraVisionIO implements VisionIO { private Supplier m_turretRotationSupplier = () -> new Rotation3d(); @@ -62,38 +64,50 @@ public void updateInputs( private void handleApriltags( VisionInputs generalInputs, ApriltagInputsAutoLogged apriltagInputs) { + if (limelightPoseEstimator.getPoseEstimate().isPresent()) { + generalInputs.timestamp = Timer.getFPGATimestamp() + - (generalInputs.captureLatency / 1000) + - (generalInputs.processLatency / 1000); - generalInputs.timestamp = - Timer.getFPGATimestamp() - - (generalInputs.captureLatency / 1000) - - (generalInputs.processLatency / 1000); - - // The camera offset is already encoded in kTurretToCameraTransform - // Transform composition handles the orbital motion automatically + // The camera offset is already encoded in kTurretToCameraTransform + // Transform composition handles the orbital motion automatically - Pose3d cameraPose = - new Pose3d() - // Robot → Turret mount - .transformBy(ShooterConstants.kRobotToTurretTransform) - // Turret rotation (e.g., yaw) - .transformBy(new Transform3d(new Translation3d(), m_turretRotationSupplier.get())) - // Turret → Camera mount point - .transformBy(ShooterConstants.kTurretToCameraTransform) - // Camera's own rotation (e.g., pitch/tilt) - .transformBy( - new Transform3d( - new Translation3d(), ShooterConstants.kTurretToCameraTransform.getRotation())); + Pose3d cameraPose = new Pose3d() + // Robot → Turret mount + .transformBy(ShooterConstants.kRobotToTurretTransform) + // Turret rotation (e.g., yaw) + .transformBy(new Transform3d(new Translation3d(), m_turretRotationSupplier.get())) + // Turret → Camera mount point + .transformBy(ShooterConstants.kTurretToCameraTransform) + // Camera's own rotation (e.g., pitch/tilt) + .transformBy( + new Transform3d( + new Translation3d(), ShooterConstants.kTurretToCameraTransform.getRotation())); - Logger.recordOutput("Vision/TurretCameraOffset", cameraPose); + Logger.recordOutput("Vision/TurretCameraOffset", cameraPose); - m_limelight.getSettings().withCameraOffset(cameraPose).save(); + m_limelight.getSettings().withCameraOffset(cameraPose).save(); - if (limelightPoseEstimator.getPoseEstimate().isPresent()) { var pose = limelightPoseEstimator.getPoseEstimate().get(); + double stdDevX = m_stdDevs.get(0, 0), stdDevY = m_stdDevs.get(1, 0), stdDevYaw = m_stdDevs.get(2, 0); + var results = m_limelight.getLatestResults(); + + if (results.isPresent()) { + double alpha = 1; // Tag Dist Gain + double beta = 0.5; // Tag Area Gain + + double tagDist = results.get().targetDistance; + double ta = results.get().ta; + Logger.recordOutput("Limelight/Gabriel/tagDist", tagDist); + stdDevX *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); + stdDevY *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); + stdDevYaw *= tagDist; + } + generalInputs.estimatedRobotPose = pose.pose.toPose2d(); - generalInputs.stdDevX = m_stdDevs.get(0, 0); - generalInputs.stdDevY = m_stdDevs.get(1, 0); - generalInputs.stdDevYaw = m_stdDevs.get(2, 0); + generalInputs.stdDevX = stdDevX; + generalInputs.stdDevY = stdDevY; + generalInputs.stdDevYaw = stdDevYaw; } else { generalInputs.estimatedRobotPose = null; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d83c321..77b6a81 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,10 +1,13 @@ package frc.robot.subsystems.vision; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; -import java.util.function.Supplier; import limelight.Limelight; import limelight.networktables.LimelightData; import limelight.networktables.LimelightPipelineData; @@ -56,17 +59,31 @@ public void updateInputs( } private void handleApriltags(VisionInputs generalInputs, ApriltagInputs apriltagInputs) { - generalInputs.timestamp = - Timer.getFPGATimestamp() - - (generalInputs.captureLatency / 1000) - - (generalInputs.processLatency / 1000); + generalInputs.timestamp = Timer.getFPGATimestamp() + - (generalInputs.captureLatency / 1000) + - (generalInputs.processLatency / 1000); if (limelightPoseEstimator.getPoseEstimate().isPresent()) { var pose = limelightPoseEstimator.getPoseEstimate().get(); + double stdDevX = m_stdDevs.get(0, 0), stdDevY = m_stdDevs.get(1, 0), stdDevYaw = m_stdDevs.get(2, 0); + var results = m_limelight.getLatestResults(); + + if (results.isPresent()) { + double alpha = 1; // Tag Dist Gain + double beta = 0.5; // Tag Area Gain + + double tagDist = results.get().targetDistance; + double ta = results.get().ta; + Logger.recordOutput("Limelight/Gabriel/tagDist", tagDist); + stdDevX *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); + stdDevY *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); + stdDevYaw *= tagDist; + } + generalInputs.estimatedRobotPose = pose.pose.toPose2d(); - generalInputs.stdDevX = m_stdDevs.get(0, 0); - generalInputs.stdDevY = m_stdDevs.get(1, 0); - generalInputs.stdDevYaw = m_stdDevs.get(2, 0); + generalInputs.stdDevX = stdDevX; + generalInputs.stdDevY = stdDevY; + generalInputs.stdDevYaw = stdDevYaw; } else { generalInputs.estimatedRobotPose = null; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 9e0f1a2..c6bc6b9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,7 +1,11 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -11,11 +15,6 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; public class VisionSubsystem extends SubsystemBase { @@ -65,13 +64,6 @@ public void periodic() { io.updateInputs(generalInputs, synapseInputs, inputs.apriltagInputs); - if (m_speedsSupplier != null) { - if (m_speedsSupplier.get().omegaRadiansPerSecond - > DegreesPerSecond.of(20).in(RadiansPerSecond)) { - return; - } - } - if (generalInputs.estimatedRobotPose != null && generalInputs.estimatedRobotPose.getX() > 0 && generalInputs.estimatedRobotPose.getY() > 0) { From 6cf3f066429890a4ed2236838485594a8cf0dd01 Mon Sep 17 00:00:00 2001 From: DanPeled Date: Fri, 29 May 2026 14:58:29 +0300 Subject: [PATCH 3/3] spotless --- .../subsystems/swerve/SwerveSubsystem.java | 88 +++++++++---------- .../vision/TurretCameraVisionIO.java | 41 +++++---- .../subsystems/vision/VisionIOLimelight.java | 17 ++-- .../subsystems/vision/VisionSubsystem.java | 12 ++- 4 files changed, 76 insertions(+), 82 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 4353291..45e373f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -3,10 +3,9 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems.swerve; -import java.io.File; -import java.io.IOException; -import java.util.Arrays; -import java.util.function.Supplier; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -17,9 +16,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meter; -import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; @@ -28,6 +24,10 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; +import java.io.File; +import java.io.IOException; +import java.util.Arrays; +import java.util.function.Supplier; import limelight.networktables.AngularVelocity3d; import limelight.networktables.Orientation3d; import robottester.RobotTester; @@ -56,16 +56,18 @@ public SwerveSubsystem(SwerveConstants.SwerveConfig config) { */ public SwerveSubsystem(File directory) { boolean blueAlliance = false; - Pose2d startingPose = blueAlliance - ? new Pose2d(new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0)) - : new Pose2d(new Translation2d(Meter.of(16), Meter.of(4)), Rotation2d.fromDegrees(180)); + Pose2d startingPose = + blueAlliance + ? new Pose2d(new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0)) + : new Pose2d(new Translation2d(Meter.of(16), Meter.of(4)), Rotation2d.fromDegrees(180)); // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary // objects being // created. SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose); + swerveDrive = + new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose); } catch (IOException e) { throw new RuntimeException(e); } @@ -85,14 +87,15 @@ public SwerveSubsystem(File directory) { } public void configureBindings(CommandXboxController driverXbox) { - SwerveInputStream driveAngularVelocity = SwerveInputStream.of( - getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(driverXbox::getRightX) - .deadband(Constants.OperatorConstants.DEADBAND) - .scaleTranslation(0.9) - .allianceRelativeControl(true); + SwerveInputStream driveAngularVelocity = + SwerveInputStream.of( + getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(driverXbox::getRightX) + .deadband(Constants.OperatorConstants.DEADBAND) + .scaleTranslation(0.9) + .allianceRelativeControl(true); Command driveFieldOrientedAnglularVelocity = driveFieldOriented(driveAngularVelocity); setDefaultCommand(driveFieldOrientedAnglularVelocity); @@ -103,13 +106,13 @@ public void configureBindings(CommandXboxController driverXbox) { public void configureTests(RobotTester tester) { tester.addTestOption( TestOption.fromCommand( - Commands.sequence( - Commands.runOnce( - () -> { - setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - }), - driveFieldOriented(() -> new ChassisSpeeds(1, 1, 0)) - .until(() -> getFieldVelocity().vyMetersPerSecond > 0.5))) + Commands.sequence( + Commands.runOnce( + () -> { + setChassisSpeeds(new ChassisSpeeds(0, 0, 0)); + }), + driveFieldOriented(() -> new ChassisSpeeds(1, 1, 0)) + .until(() -> getFieldVelocity().vyMetersPerSecond > 0.5))) .withTestTimeout(Seconds.of(2)) .withCleanup( () -> { @@ -124,12 +127,10 @@ public void configureTests(RobotTester tester) { } @Override - public void periodic() { - } + public void periodic() {} @Override - public void simulationPeriodic() { - } + public void simulationPeriodic() {} /** * Command to characterize the robot drive motors using SysId @@ -209,10 +210,8 @@ public SwerveDriveKinematics getKinematics() { } /** - * Resets odometry to the given pose. Gyro angle and module positions do not - * need to be reset when - * calling this method. However, if either gyro angle or module position is - * reset, this must be + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when + * calling this method. However, if either gyro angle or module position is reset, this must be * called in order for odometry to keep working. * * @param initialHolonomicPose The pose to set the odometry to @@ -222,8 +221,7 @@ public void resetOdometry(Pose2d initialHolonomicPose) { } /** - * Gets the current pose (position and rotation) of the robot, as reported by - * odometry. + * Gets the current pose (position and rotation) of the robot, as reported by odometry. * * @return The robot's pose */ @@ -250,8 +248,7 @@ public void postTrajectory(Trajectory trajectory) { } /** - * Resets the gyro angle to zero and resets odometry to the same position, but - * facing toward 0. + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. */ public void zeroGyro() { swerveDrive.zeroGyro(); @@ -260,8 +257,7 @@ public void zeroGyro() { /** * Checks if the alliance is red, defaults to false if alliance isn't available. * - * @return true if the red alliance, false if blue. Defaults to false if none is - * available. + * @return true if the red alliance, false if blue. Defaults to false if none is available. */ private boolean isRedAlliance() { var alliance = DriverStation.getAlliance(); @@ -269,11 +265,9 @@ private boolean isRedAlliance() { } /** - * This will zero (calibrate) the robot to assume the current position is facing - * forward + * This will zero (calibrate) the robot to assume the current position is facing forward * - *

- * If red alliance rotate the robot 180 after the drviebase zero command + *

If red alliance rotate the robot 180 after the drviebase zero command */ public void zeroGyroWithAlliance() { if (isRedAlliance()) { @@ -295,10 +289,8 @@ public void setMotorBrake(boolean brake) { } /** - * Gets the current yaw angle of the robot, as reported by the swerve pose - * estimator in the - * underlying drivebase. Note, this is not the raw gyro reading, this may be - * corrected from calls + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the + * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls * to resetOdometry(). * * @return The yaw angle diff --git a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java index b1a00b2..7ea93a1 100644 --- a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java @@ -1,9 +1,5 @@ package frc.robot.subsystems.vision; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -13,11 +9,13 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.shooter.ShooterConstants; +import java.util.function.Supplier; import limelight.Limelight; import limelight.networktables.LimelightData; import limelight.networktables.LimelightPipelineData; import limelight.networktables.LimelightPoseEstimator; import limelight.networktables.LimelightPoseEstimator.EstimationMode; +import org.littletonrobotics.junction.Logger; public class TurretCameraVisionIO implements VisionIO { private Supplier m_turretRotationSupplier = () -> new Rotation3d(); @@ -65,31 +63,36 @@ public void updateInputs( private void handleApriltags( VisionInputs generalInputs, ApriltagInputsAutoLogged apriltagInputs) { if (limelightPoseEstimator.getPoseEstimate().isPresent()) { - generalInputs.timestamp = Timer.getFPGATimestamp() - - (generalInputs.captureLatency / 1000) - - (generalInputs.processLatency / 1000); + generalInputs.timestamp = + Timer.getFPGATimestamp() + - (generalInputs.captureLatency / 1000) + - (generalInputs.processLatency / 1000); // The camera offset is already encoded in kTurretToCameraTransform // Transform composition handles the orbital motion automatically - Pose3d cameraPose = new Pose3d() - // Robot → Turret mount - .transformBy(ShooterConstants.kRobotToTurretTransform) - // Turret rotation (e.g., yaw) - .transformBy(new Transform3d(new Translation3d(), m_turretRotationSupplier.get())) - // Turret → Camera mount point - .transformBy(ShooterConstants.kTurretToCameraTransform) - // Camera's own rotation (e.g., pitch/tilt) - .transformBy( - new Transform3d( - new Translation3d(), ShooterConstants.kTurretToCameraTransform.getRotation())); + Pose3d cameraPose = + new Pose3d() + // Robot → Turret mount + .transformBy(ShooterConstants.kRobotToTurretTransform) + // Turret rotation (e.g., yaw) + .transformBy(new Transform3d(new Translation3d(), m_turretRotationSupplier.get())) + // Turret → Camera mount point + .transformBy(ShooterConstants.kTurretToCameraTransform) + // Camera's own rotation (e.g., pitch/tilt) + .transformBy( + new Transform3d( + new Translation3d(), + ShooterConstants.kTurretToCameraTransform.getRotation())); Logger.recordOutput("Vision/TurretCameraOffset", cameraPose); m_limelight.getSettings().withCameraOffset(cameraPose).save(); var pose = limelightPoseEstimator.getPoseEstimate().get(); - double stdDevX = m_stdDevs.get(0, 0), stdDevY = m_stdDevs.get(1, 0), stdDevYaw = m_stdDevs.get(2, 0); + double stdDevX = m_stdDevs.get(0, 0), + stdDevY = m_stdDevs.get(1, 0), + stdDevYaw = m_stdDevs.get(2, 0); var results = m_limelight.getLatestResults(); if (results.isPresent()) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 77b6a81..e015b87 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,19 +1,17 @@ package frc.robot.subsystems.vision; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; +import java.util.function.Supplier; import limelight.Limelight; import limelight.networktables.LimelightData; import limelight.networktables.LimelightPipelineData; import limelight.networktables.LimelightPoseEstimator; import limelight.networktables.LimelightPoseEstimator.EstimationMode; import limelight.networktables.Orientation3d; +import org.littletonrobotics.junction.Logger; public class VisionIOLimelight implements VisionIO { @@ -59,13 +57,16 @@ public void updateInputs( } private void handleApriltags(VisionInputs generalInputs, ApriltagInputs apriltagInputs) { - generalInputs.timestamp = Timer.getFPGATimestamp() - - (generalInputs.captureLatency / 1000) - - (generalInputs.processLatency / 1000); + generalInputs.timestamp = + Timer.getFPGATimestamp() + - (generalInputs.captureLatency / 1000) + - (generalInputs.processLatency / 1000); if (limelightPoseEstimator.getPoseEstimate().isPresent()) { var pose = limelightPoseEstimator.getPoseEstimate().get(); - double stdDevX = m_stdDevs.get(0, 0), stdDevY = m_stdDevs.get(1, 0), stdDevYaw = m_stdDevs.get(2, 0); + double stdDevX = m_stdDevs.get(0, 0), + stdDevY = m_stdDevs.get(1, 0), + stdDevYaw = m_stdDevs.get(2, 0); var results = m_limelight.getLatestResults(); if (results.isPresent()) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index c6bc6b9..3b6a8aa 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,12 +1,5 @@ package frc.robot.subsystems.vision; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.VecBuilder; @@ -15,6 +8,11 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; public class VisionSubsystem extends SubsystemBase {