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..45e373f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -61,7 +61,8 @@ public SwerveSubsystem(File directory) { ? 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 { @@ -80,7 +81,8 @@ 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 } @@ -329,6 +331,13 @@ public void lock() { swerveDrive.lockPose(); } + public Command lockPose() { + return runOnce( + () -> { + 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..7ea93a1 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; @@ -63,38 +62,55 @@ public void updateInputs( private void handleApriltags( VisionInputs generalInputs, ApriltagInputsAutoLogged apriltagInputs) { - - 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())); - - Logger.recordOutput("Vision/TurretCameraOffset", cameraPose); - - m_limelight.getSettings().withCameraOffset(cameraPose).save(); - if (limelightPoseEstimator.getPoseEstimate().isPresent()) { + 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())); + + 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); + 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/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 } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d83c321..e015b87 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -11,6 +11,7 @@ import limelight.networktables.LimelightPoseEstimator; import limelight.networktables.LimelightPoseEstimator.EstimationMode; import limelight.networktables.Orientation3d; +import org.littletonrobotics.junction.Logger; public class VisionIOLimelight implements VisionIO { @@ -63,10 +64,27 @@ private void handleApriltags(VisionInputs generalInputs, ApriltagInputs apriltag 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..3b6a8aa 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,8 +1,5 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.VecBuilder; @@ -65,13 +62,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) {