From f6a1bcfcae8304dbfc4a906235589a6991a66a71 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 23 Mar 2026 12:08:52 -0400 Subject: [PATCH 01/10] Remove climb --- .../java/frc/robot/Commands/ClimbCommand.java | 69 ------------------- src/main/java/frc/robot/RobotContainer.java | 1 - .../java/frc/robot/config/CANMappings.java | 19 +++-- src/main/java/frc/robot/subsystems/Climb.java | 69 ------------------- 4 files changed, 9 insertions(+), 149 deletions(-) delete mode 100644 src/main/java/frc/robot/Commands/ClimbCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/Climb.java diff --git a/src/main/java/frc/robot/Commands/ClimbCommand.java b/src/main/java/frc/robot/Commands/ClimbCommand.java deleted file mode 100644 index e73d216..0000000 --- a/src/main/java/frc/robot/Commands/ClimbCommand.java +++ /dev/null @@ -1,69 +0,0 @@ -package frc.robot.Commands; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.ClimbConfig; -import frc.robot.subsystems.Climb; - -@Logged -public class ClimbCommand extends Command { - public static enum Position { - DIRECTIONAL_CLIMB, - DIRECTIONAL_UNCLIMB, - POSITIONAL_CLIMB, - POSITIONAL_UNCLIMB - } - - private Climb subsystem; - private Position pose; - - public ClimbCommand(Climb subsystem, Position pose) { - this.pose = pose; - this.subsystem = subsystem; - - addRequirements(subsystem); - } - - @Override - public void initialize() { - switch (pose) { - - // Pulls down on climber (moves climber in down direction) - case DIRECTIONAL_CLIMB: - subsystem.retractDirectional(ClimbConfig.CLIMB_CLIMB_SPEED); - System.out.println("Climb: Directional Climb"); - break; - - // Pushes up on climber (moves climber in up direction) - case DIRECTIONAL_UNCLIMB: - subsystem.extendDirectional(ClimbConfig.CLIMB_UNCLIMB_SPEED); - System.out.println("Climb: Directional Unclimb"); - break; - - // Moves climber to climb position (pulls robot up to climb position) - case POSITIONAL_CLIMB: - subsystem.move(ClimbConfig.CLIMB_CLIMB_ROTATION); - System.out.println("Climb: Positional Climb"); - break; - - // Moves climber to unclimb position (lets robot back down to climb position) - case POSITIONAL_UNCLIMB: - subsystem.move(ClimbConfig.CLIMB_UNCLIMB_ROTATION); - System.out.println("Climb: Positional Unclimb"); - break; - - default: - break; - } - } - - @Override - public void end(boolean interrupted) { - subsystem.stop(); - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87df988..86abbc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,7 +22,6 @@ public class RobotContainer { private Flywheel flywheel = new Flywheel(); private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Hopper hopper = new Hopper(); - private Climb climb = new Climb(); private Intake intake = new Intake(); private Kicker kicker = new Kicker(); private CommandXboxController controller = new CommandXboxController(0); diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index 36debfd..5659ca6 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -1,16 +1,15 @@ package frc.robot.config; public class CANMappings { - public static final int SPINDEXER_MOTOR_ID = 0; - public static final int HOPPER_MOTOR_ID = 0; - public static final int KICKER_FRONT_MOTOR_ID = 0; - public static final int KICKER_BACK_MOTOR_ID = 0; - public static final int INTAKE_MOTOR_ID = 0; - public static final int CLIMB_MOTOR_ID = 3; - public static final int TURRET_MOTOR_ID = 1; - public static final int HOOD_MOTOR_ID = 0; - public static final int FLYWHEEL_RIGHT_MOTOR_ID = 0; - public static final int FLYWHEEL_LEFT_MOTOR_ID = 3; + public static final int HOPPER_MOTOR_ID = 1; + + public static final int KICKER_FRONT_MOTOR_ID = 15; // Bottom + public static final int KICKER_BACK_MOTOR_ID = 16; + + public static final int INTAKE_MOTOR_ID = 3; + + public static final int FLYWHEEL_RIGHT_MOTOR_ID = 19; + public static final int FLYWHEEL_LEFT_MOTOR_ID = 2; public static final int PIGEON_CAN_ID = 0; } diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java deleted file mode 100644 index 116eda1..0000000 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ /dev/null @@ -1,69 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.config.CANMappings; -import frc.robot.config.ClimbConfig; - -@Logged -public class Climb extends SubsystemBase { - protected TalonFX climb; - - public Climb() { - climb = new TalonFX(CANMappings.CLIMB_MOTOR_ID); - - TalonFXConfiguration climbConfig = new TalonFXConfiguration(); - - climbConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - climbConfig.CurrentLimits.StatorCurrentLimitEnable = true; - climbConfig.CurrentLimits.StatorCurrentLimit = ClimbConfig.CLIMB_STATOR_CURRENT_LIMIT; - - climbConfig.CurrentLimits.SupplyCurrentLimit = ClimbConfig.CLIMB_SUPPLY_CURRENT_LIMIT; - - climbConfig.MotionMagic.MotionMagicCruiseVelocity = ClimbConfig.CLIMB_MAX_CRUISE_VELOCITY; - climbConfig.MotionMagic.MotionMagicAcceleration = ClimbConfig.CLIMB_TARGET_ACCELERATION; - - climbConfig.Slot0.kP = ClimbConfig.CLIMB_P; - climbConfig.Slot0.kI = ClimbConfig.CLIMB_I; - climbConfig.Slot0.kD = ClimbConfig.CLIMB_D; - - climbConfig.Feedback.SensorToMechanismRatio = ClimbConfig.CLIMB_GEAR_RATIO; - climbConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - climbConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - climb.getConfigurator().apply(climbConfig); - } - - public void move(double rotation) { - climb.setControl(new MotionMagicVoltage(rotation)); - } - - // Set negatives and positives - public void extendDirectional(double speed) { - speed = Math.abs(speed); - climb.setControl(new DutyCycleOut(speed)); - } - - public void retractDirectional(double speed) { - speed = Math.abs(speed); - climb.setControl(new DutyCycleOut(speed)); - } - - public void stop() { - climb.stopMotor(); - } - - public void zero() { - climb.setPosition(0.0); - } - - public boolean atPosition() { - return (Math.abs(climb.getClosedLoopError().getValueAsDouble()) <= ClimbConfig.CLIMB_TOLERANCE); - } -} From 27052e404e1297cfe11e3708f4e4767fc3eb593b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 23 Mar 2026 12:32:52 -0400 Subject: [PATCH 02/10] Remove climb --- src/main/java/frc/robot/RobotContainer.java | 1 - .../java/frc/robot/commands/ClimbCommand.java | 63 ------------------- .../java/frc/robot/config/ClimbConfig.java | 22 ------- src/main/java/frc/robot/subsystems/Climb.java | 63 ------------------- 4 files changed, 149 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ClimbCommand.java delete mode 100644 src/main/java/frc/robot/config/ClimbConfig.java delete mode 100644 src/main/java/frc/robot/subsystems/Climb.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 64a51bd..f854b3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,7 +28,6 @@ public class RobotContainer { private Flywheel flywheel = new Flywheel(); private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Hopper hopper = new Hopper(); - private Climb climb = new Climb(); private Intake intake = new Intake(); private Kicker kicker = new Kicker(); private CommandXboxController controller = new CommandXboxController(1); diff --git a/src/main/java/frc/robot/commands/ClimbCommand.java b/src/main/java/frc/robot/commands/ClimbCommand.java deleted file mode 100644 index e38a0b0..0000000 --- a/src/main/java/frc/robot/commands/ClimbCommand.java +++ /dev/null @@ -1,63 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.Command; - -@Logged -public class ClimbCommand extends Command { - // public static enum Position { - // DIRECTIONAL_CLIMB, - // DIRECTIONAL_UNCLIMB, - // POSITIONAL_CLIMB, - // POSITIONAL_UNCLIMB - // } - // - // private Climb subsystem; - // private Position pose; - // - // public ClimbCommand(Climb subsystem, Position pose) { - // this.pose = pose; - // this.subsystem = subsystem; - // - // addRequirements(subsystem); - // } - // - // @Override - // public void initialize() { - // switch (pose) { - // - // // Pulls down on climber (moves climber in down direction) - // case DIRECTIONAL_CLIMB: - // subsystem.retractDirectional(ClimbConfig.CLIMB_CLIMB_SPEED); - // break; - // - // // Pushes up on climber (moves climber in up direction) - // case DIRECTIONAL_UNCLIMB: - // subsystem.extendDirectional(ClimbConfig.CLIMB_UNCLIMB_SPEED); - // break; - // - // // Moves climber to climb position (pulls robot up to climb position) - // case POSITIONAL_CLIMB: - // subsystem.move(ClimbConfig.CLIMB_CLIMB_ROTATION); - // break; - // - // // Moves climber to unclimb position (lets robot back down to climb position) - // case POSITIONAL_UNCLIMB: - // subsystem.move(ClimbConfig.CLIMB_UNCLIMB_ROTATION); - // break; - // - // default: - // break; - // } - // } - // - // @Override - // public void end(boolean interrupted) { - // subsystem.stop(); - // } - // - // @Override - // public boolean isFinished() { - // return false; - // } -} diff --git a/src/main/java/frc/robot/config/ClimbConfig.java b/src/main/java/frc/robot/config/ClimbConfig.java deleted file mode 100644 index a10d297..0000000 --- a/src/main/java/frc/robot/config/ClimbConfig.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.config; - -public class ClimbConfig { - public static final double CLIMB_SUPPLY_CURRENT_LIMIT = 70; - public static final double CLIMB_STATOR_CURRENT_LIMIT = 80; - public static final double CLIMB_GEAR_RATIO = 1; - - // Set - public static final double CLIMB_MAX_CRUISE_VELOCITY = 3000; - public static final double CLIMB_TARGET_ACCELERATION = 500; - public static final double CLIMB_P = 10; - public static final double CLIMB_I = 0; - public static final double CLIMB_D = 0; - - public static final double CLIMB_TOLERANCE = 0.01; - - // Set - public static final double CLIMB_UNCLIMB_ROTATION = 0; - public static final double CLIMB_CLIMB_ROTATION = 0; - public static final double CLIMB_UNCLIMB_SPEED = 0; - public static final double CLIMB_CLIMB_SPEED = 0; -} diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java deleted file mode 100644 index fde908c..0000000 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ /dev/null @@ -1,63 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -@Logged -public class Climb extends SubsystemBase { - protected TalonFX climb; - - // public Climb() { - // climb = new TalonFX(CANMappings.CLIMB_MOTOR_ID); - // - // TalonFXConfiguration climbConfig = new TalonFXConfiguration(); - // - // climbConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - // climbConfig.CurrentLimits.StatorCurrentLimitEnable = true; - // climbConfig.CurrentLimits.StatorCurrentLimit = ClimbConfig.CLIMB_STATOR_CURRENT_LIMIT; - // - // climbConfig.CurrentLimits.SupplyCurrentLimit = ClimbConfig.CLIMB_SUPPLY_CURRENT_LIMIT; - // - // climbConfig.MotionMagic.MotionMagicCruiseVelocity = ClimbConfig.CLIMB_MAX_CRUISE_VELOCITY; - // climbConfig.MotionMagic.MotionMagicAcceleration = ClimbConfig.CLIMB_TARGET_ACCELERATION; - // - // climbConfig.Slot0.kP = ClimbConfig.CLIMB_P; - // climbConfig.Slot0.kI = ClimbConfig.CLIMB_I; - // climbConfig.Slot0.kD = ClimbConfig.CLIMB_D; - // - // climbConfig.Feedback.SensorToMechanismRatio = ClimbConfig.CLIMB_GEAR_RATIO; - // climbConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - // climbConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - // - // climb.getConfigurator().apply(climbConfig); - // } - // - // public void move(double rotation) { - // climb.setControl(new MotionMagicVoltage(rotation)); - // } - // - // // Set negatives and positives - // public void extendDirectional(double speed) { - // speed = Math.abs(speed); - // climb.setControl(new DutyCycleOut(speed)); - // } - // - // public void retractDirectional(double speed) { - // speed = Math.abs(speed); - // climb.setControl(new DutyCycleOut(speed)); - // } - // - // public void stop() { - // climb.stopMotor(); - // } - // - // public void zero() { - // climb.setPosition(0.0); - // } - // - // public boolean atPosition() { - // return (Math.abs(climb.getClosedLoopError().getValueAsDouble()) <= - // ClimbConfig.CLIMB_TOLERANCE); - // } -} From 6ce3996cc6801b0d26c1f9d77b90155c3760c8a8 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 23 Mar 2026 12:44:33 -0400 Subject: [PATCH 03/10] add vision --- src/main/java/frc/robot/Robot.java | 58 ++++++++++++------- .../java/frc/robot/config/VisionConfig.java | 23 ++++++-- .../java/frc/robot/subsystems/Vision.java | 6 +- 3 files changed, 59 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c226996..efad183 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,28 +5,29 @@ package frc.robot; import static frc.robot.RobotContainer.shooterState; -import static frc.robot.config.VisionConfig.photonPoseEstimatorForward; -import static frc.robot.config.VisionConfig.result; import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.FlywheelCommand; import frc.robot.config.TunerConstants; -import frc.robot.config.VisionConfig; import frc.robot.subsystems.CommandSwerveDrivetrain; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; @Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + // last time we injected a synthetic vision measurement (seconds, FPGA time) + private double m_lastSimVisionTime = 0.0; + // whether we've injected a one-time offset vision measurement for testing + private boolean m_injectedOffset = false; private final RobotContainer m_robotContainer; @@ -39,22 +40,19 @@ public Robot() { @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); - Optional visionEst = - VisionConfig.photonPoseEstimatorForward.estimateCoprocMultiTagPose(result); - if (visionEst.isEmpty()) { - visionEst = VisionConfig.photonPoseEstimatorForward.estimateLowestAmbiguityPose(result); - } else { - drivetrain.addVisionMeasurement( - photonPoseEstimatorForward - .estimateAverageBestTargetsPose(result) - .get() - .estimatedPose - .toPose2d(), - visionEst.get().timestampSeconds); - } SmartDashboard.putString("shoot-state", shooterState); SmartDashboard.putString("shoot-on", FlywheelCommand.isOn); + try { + var pose = drivetrain.getState().Pose; + SmartDashboard.putNumber("OdometryX", pose.getX()); + SmartDashboard.putNumber("OdometryY", pose.getY()); + SmartDashboard.putNumber("OdometryRotDeg", pose.getRotation().getDegrees()); + SmartDashboard.putNumber("SimVisionLastInject", m_lastSimVisionTime); + System.out.printf( + "ODOM X=%.3f Y=%.3f R=%.2f SimVision=%.3f\n", + pose.getX(), pose.getY(), pose.getRotation().getDegrees(), m_lastSimVisionTime); + } catch (Exception ignored) { + } } @Override @@ -109,5 +107,25 @@ public void testExit() {} public void simulationInit() {} @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + double now = Timer.getFPGATimestamp(); + if (now - m_lastSimVisionTime >= 0.1) { + m_lastSimVisionTime = now; + try { + var truePose = drivetrain.getState().Pose; + // After 3 seconds, inject a single offset measurement (+1m X) to observe correction + if (!m_injectedOffset && now > 3.0) { + Pose2d offsetPose = + new Pose2d(truePose.getX() + 1.0, truePose.getY(), truePose.getRotation()); + drivetrain.addVisionMeasurement(offsetPose, now); + System.out.println("SIM INJECT: OFFSET +1.0m X at " + now); + m_injectedOffset = true; + } else { + drivetrain.addVisionMeasurement(truePose, now); + } + } catch (Exception e) { + DriverStation.reportError("Sim vision injection failed: " + e.getMessage(), false); + } + } + } } diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index 27429e1..f52fc0b 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -2,17 +2,25 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator; import org.photonvision.targeting.PhotonPipelineResult; public class VisionConfig { public static PhotonPipelineResult result = new PhotonPipelineResult(); - public static final String CAMERA_NAME = "forwardAprilTag"; // left + public static final String FRONT_CAMERA_NAME = "forwardAprilTag"; + // Per-camera measurement standard deviations: [x (m), y (m), theta (rad)] + public static final Matrix FRONT_VISION_STDDEVS = + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(3.0)); + public static final Matrix REAR_VISION_STDDEVS = + VecBuilder.fill(0.10, 0.10, Units.degreesToRadians(5.0)); + public static final String REAR_CAMERA_NAME = "rearAprilTag"; public static final AprilTagFieldLayout LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); public static final PhotonPoseEstimator.PoseStrategy STRATEGY = @@ -23,7 +31,10 @@ public class VisionConfig { Units.inchesToMeters(-11.55), Units.inchesToMeters(14.88), new Rotation3d(0.0, Units.degreesToRadians(16), 0.0)); - Optional visionEst = Optional.empty(); - public static PhotonPoseEstimator photonPoseEstimatorForward = - new PhotonPoseEstimator(LAYOUT, FORWARD_CAMERA_POSITION); + public static final Transform3d REAR_CAMERA_POSITION = + new Transform3d( + Units.inchesToMeters(0.0), + Units.inchesToMeters(0.0), + Units.inchesToMeters(0.0), + new Rotation3d(0.0, Units.degreesToRadians(0.0), 0.0)); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index c98952e..7918de2 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,8 +3,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.VisionConfig; import org.photonvision.PhotonCamera; -import org.photonvision.estimation.*; public class Vision extends SubsystemBase { - public static final PhotonCamera FrontCameraApril = new PhotonCamera(VisionConfig.CAMERA_NAME); + public static final PhotonCamera FrontCameraApril = + new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); + public static final PhotonCamera RearCameraApril = + new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); } From b2fa1c850401cfeea502340fd82e58bec249ffa5 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 23 Mar 2026 12:44:44 -0400 Subject: [PATCH 04/10] add vision --- .../subsystems/CommandSwerveDrivetrain.java | 85 +++++++++++++++++++ 1 file changed, 85 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 8c37108..9fa0962 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -11,6 +11,8 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -26,8 +28,10 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.config.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.config.VisionConfig; import java.util.Optional; import java.util.function.Supplier; +import org.photonvision.PhotonPoseEstimator; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily @@ -38,6 +42,8 @@ */ @Logged public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { + private PhotonPoseEstimator m_FrontPhotonPoseEstimator; + private PhotonPoseEstimator m_RearPhotonPoseEstimator; private static final double kSimLoopPeriod = 0.004; // 4 ms private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -129,6 +135,7 @@ public CommandSwerveDrivetrain( startSimThread(); } configureAutoBuilder(); + initVisionPoseEstimator(); } /** @@ -151,6 +158,7 @@ public CommandSwerveDrivetrain( startSimThread(); } configureAutoBuilder(); + initVisionPoseEstimator(); } /** @@ -180,6 +188,7 @@ public CommandSwerveDrivetrain( odometryStandardDeviation, visionStandardDeviation, modules); + initVisionPoseEstimator(); if (Utils.isSimulation()) { startSimThread(); } @@ -216,6 +225,29 @@ private void configureAutoBuilder() { } } + private void initVisionPoseEstimator() { + try { + AprilTagFieldLayout fieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + this.m_FrontPhotonPoseEstimator = + new PhotonPoseEstimator(fieldLayout, VisionConfig.FORWARD_CAMERA_POSITION); + } catch (Exception e) { + DriverStation.reportError( + "Failed to create front PhotonPoseEstimator: " + e.getMessage(), true); + this.m_FrontPhotonPoseEstimator = null; + } + try { + AprilTagFieldLayout fieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + this.m_RearPhotonPoseEstimator = + new PhotonPoseEstimator(fieldLayout, VisionConfig.REAR_CAMERA_POSITION); + } catch (Exception e) { + DriverStation.reportError( + "Failed to create rear PhotonPoseEstimator: " + e.getMessage(), true); + this.m_RearPhotonPoseEstimator = null; + } + } + /** * Returns a command that applies the specified control request to this swerve drivetrain. * @@ -268,6 +300,59 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + // 1. Get all results since the last loop and apply if estimator is available + if (m_FrontPhotonPoseEstimator != null) { + var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); + + for (var result : frontResults) { + // 2. Use the 2026 explicit methods to calculate pose + var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + + // Fallback to single tag if multi-tag isn't available + if (visionEst.isEmpty()) { + var bestTarget = result.getBestTarget(); + if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { + visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); + } + } + + // 3. Apply the successful estimation to the CTRE odometry and log to file + visionEst.ifPresent( + est -> { + Pose2d pose = est.estimatedPose.toPose2d(); + double ts = est.timestampSeconds; + SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); + SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); + addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); + }); + } + } + if (m_RearPhotonPoseEstimator != null) { + var rearResults = Vision.RearCameraApril.getAllUnreadResults(); + + for (var result : rearResults) { + // 2. Use the 2026 explicit methods to calculate pose + var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + + // Fallback to single tag if multi-tag isn't available + if (visionEst.isEmpty()) { + var bestTarget = result.getBestTarget(); + if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { + visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); + } + } + + // 3. Apply the successful estimation to the CTRE odometry and log to file + visionEst.ifPresent( + est -> { + Pose2d pose = est.estimatedPose.toPose2d(); + double ts = est.timestampSeconds; + SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); + SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); + addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); + }); + } + } } private void startSimThread() { From 13d70b9d530ab8027e19610ca7c34c571d05708f Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 23 Mar 2026 12:45:02 -0400 Subject: [PATCH 05/10] updated stuff --- .idea/modules.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/.idea/modules.xml b/.idea/modules.xml index d0ddc74..73a3b4f 100644 --- a/.idea/modules.xml +++ b/.idea/modules.xml @@ -2,6 +2,7 @@ + From 606ae5c4320085dea7fc0cfbc13c7ba683664617 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 25 Mar 2026 06:51:13 -0400 Subject: [PATCH 06/10] auto align tested --- src/main/java/frc/robot/Robot.java | 17 ++-- src/main/java/frc/robot/RobotContainer.java | 14 +++- .../frc/robot/commands/DrivetrainCommand.java | 84 ++++++++++++++++++- .../frc/robot/helpers/ShotCalculator.java | 16 +++- 4 files changed, 117 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efad183..04920b1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,9 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; @@ -28,8 +31,12 @@ public class Robot extends TimedRobot { private double m_lastSimVisionTime = 0.0; // whether we've injected a one-time offset vision measurement for testing private boolean m_injectedOffset = false; - + private Pose2d robotPose = new Pose2d(); private final RobotContainer m_robotContainer; + StructPublisher publisher = + NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish(); + StructArrayPublisher arrayPublisher = + NetworkTableInstance.getDefault().getStructArrayTopic("MyPoseArray", Pose2d.struct).publish(); public Robot() { m_robotContainer = new RobotContainer(); @@ -40,6 +47,7 @@ public Robot() { @Override public void robotPeriodic() { + CommandScheduler.getInstance().run(); SmartDashboard.putString("shoot-state", shooterState); SmartDashboard.putString("shoot-on", FlywheelCommand.isOn); try { @@ -48,11 +56,11 @@ public void robotPeriodic() { SmartDashboard.putNumber("OdometryY", pose.getY()); SmartDashboard.putNumber("OdometryRotDeg", pose.getRotation().getDegrees()); SmartDashboard.putNumber("SimVisionLastInject", m_lastSimVisionTime); - System.out.printf( - "ODOM X=%.3f Y=%.3f R=%.2f SimVision=%.3f\n", - pose.getX(), pose.getY(), pose.getRotation().getDegrees(), m_lastSimVisionTime); } catch (Exception ignored) { } + robotPose = drivetrain.getState().Pose; + publisher.set(robotPose); + arrayPublisher.set(new Pose2d[] {robotPose}); } @Override @@ -118,7 +126,6 @@ public void simulationPeriodic() { Pose2d offsetPose = new Pose2d(truePose.getX() + 1.0, truePose.getY(), truePose.getRotation()); drivetrain.addVisionMeasurement(offsetPose, now); - System.out.println("SIM INJECT: OFFSET +1.0m X at " + now); m_injectedOffset = true; } else { drivetrain.addVisionMeasurement(truePose, now); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f854b3b..ae3efca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,7 +5,6 @@ package frc.robot; import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; @@ -31,7 +30,6 @@ public class RobotContainer { private Intake intake = new Intake(); private Kicker kicker = new Kicker(); private CommandXboxController controller = new CommandXboxController(1); - private SwerveDriveState driveState = new SwerveDriveState(); private final SendableChooser autoChooser; public static boolean isExtended = false; public static String shooterState = "none"; @@ -121,6 +119,16 @@ private void configureBindings() { controller::getLeftY, controller::getRightX)); + controller + .x() + .toggleOnTrue( + new DrivetrainCommand( + drivetrain, + DrivetrainCommand.Position.AUTO_ALIGN_HUB, + controller::getLeftX, + controller::getLeftY, + controller::getRightX)); + /* Controls */ // Y = Shoot Toggle controller @@ -175,8 +183,6 @@ private void configureBindings() { .alongWith(Commands.run(() -> shooterState = "TRENCH"))); // Down Plus = Zero hopper controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper)); - // Up Plus = Defense Hopper - controller.povUp().toggleOnTrue(Commands.run(() -> hopper.retractDirectional(0.8), hopper)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index c59c695..07bd72d 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -5,11 +5,15 @@ import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.config.TunerConstants; import frc.robot.helpers.ApplyModuleStates; +import frc.robot.helpers.ShotCalculator; import frc.robot.subsystems.CommandSwerveDrivetrain; import java.util.function.DoubleSupplier; @@ -18,7 +22,8 @@ public class DrivetrainCommand extends Command { public static enum Position { TELEOP, STILL_SHOT, - SOTM + SOTM, + AUTO_ALIGN_HUB } private CommandSwerveDrivetrain subsystem; @@ -48,6 +53,8 @@ public static enum Position { }; private final ApplyModuleStates applyRequest = new ApplyModuleStates(); private final SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake(); + private final ProfiledPIDController autoAlignPidController; + private Rotation2d m_targetAngle = Rotation2d.kZero; public DrivetrainCommand( CommandSwerveDrivetrain subsystem, @@ -60,10 +67,38 @@ public DrivetrainCommand( this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; + double alignP = 1; + double alignI = 0; + double alignD = 0; + this.autoAlignPidController = + new ProfiledPIDController( + alignP, + alignI, + alignD, + new TrapezoidProfile.Constraints(MaxAngularRate, MaxAngularRate / 0.2)); + this.autoAlignPidController.enableContinuousInput( + -Math.PI, Math.PI); // Swerve angles are continuous (-180 to 180 deg) + // Don't compute hub-facing target in the constructor (DriverStation alliance may be + // unavailable during initialization). Initialize to current heading; execute() will + // recompute the actual hub-facing target each loop. + this.m_targetAngle = subsystem.getState().Pose.getRotation(); addRequirements(subsystem); } + public double getAutoAlignRotationalOutput() { + Rotation2d currentAngle = subsystem.getState().Pose.getRotation(); + double current = currentAngle.getRadians(); + double target = m_targetAngle.getRadians(); + double output = autoAlignPidController.calculate(current, target); + if (output > MaxAngularRate) { + output = MaxAngularRate; + } else if (output < -MaxAngularRate) { + output = -MaxAngularRate; + } + return output; + } + @Override public void execute() { switch (pose) { @@ -78,6 +113,7 @@ public void execute() { .withRotationalRate( -rightX.getAsDouble() * MaxAngularRate)); // Drive counterclockwise with negative X + System.out.println("TELEOP"); break; // (Needs check) Mode for when shots are from a still position @@ -94,6 +130,34 @@ public void execute() { System.out.println("Drivetrain: SOTM Drive"); break; + case AUTO_ALIGN_HUB: + System.out.println("AUTO ALIGN"); + // Recompute the desired heading toward the hub each loop + m_targetAngle = + ShotCalculator.getRotationTowardsHub( + ShotCalculator.calculateHubPosition(), subsystem.getState().Pose.getTranslation()); + double currentRad = subsystem.getState().Pose.getRotation().getRadians(); + double targetRad = m_targetAngle.getRadians(); + // Normalize error to [-pi, pi] + double error = + Math.atan2(Math.sin(targetRad - currentRad), Math.cos(targetRad - currentRad)); + double absError = Math.abs(error); + double angleTolerance = Math.toRadians(1.0); // stop within 1 degree + + double rotOutput = getAutoAlignRotationalOutput(); + + if (absError < angleTolerance) { + // Aligned: stop rotating (and hold position) + subsystem.setControl(drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0)); + } else { + // Not aligned: apply rotational output + System.out.println("new rotational output: " + rotOutput); + subsystem.setControl( + drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput)); + } + + break; + default: break; } @@ -104,6 +168,22 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return false; + if (pose != Position.AUTO_ALIGN_HUB) { + return false; + } + + Translation2d hubPos = ShotCalculator.calculateHubPosition(); + + Translation2d currentTrans = subsystem.getState().Pose.getTranslation(); + if (currentTrans == null) { + return false; + } + + Rotation2d target = ShotCalculator.getRotationTowardsHub(hubPos, currentTrans); + double current = subsystem.getState().Pose.getRotation().getRadians(); + double targetRad = target.getRadians(); + double err = Math.atan2(Math.sin(targetRad - current), Math.cos(targetRad - current)); + double angleTolerance = Math.toRadians(1.0); // 1 degree + return Math.abs(err) < angleTolerance; } } diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index adae354..10085ea 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -1,5 +1,6 @@ package frc.robot.helpers; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.util.Units; @@ -11,7 +12,7 @@ public class ShotCalculator { /* Calculation Methods */ public static double calculateFlywheelShot(Translation2d robotPosition) { // 1. Calculate distance between goal and robot position - double distanceToGoal = calculateGoalPosition().getDistance(robotPosition); + double distanceToGoal = calculateHubPosition().getDistance(robotPosition); // 2. Retrieve correlating RPS from map return DISTANCE_TO_FLYWHEEL.get(distanceToGoal); } @@ -27,7 +28,7 @@ public static double calculateFlywheelDefaultShot(Translation2d robotPosition) { } /* Helper Methods */ - public static Translation2d calculateGoalPosition() { + public static Translation2d calculateHubPosition() { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { if (alliance.get() == DriverStation.Alliance.Blue) { @@ -37,7 +38,16 @@ public static Translation2d calculateGoalPosition() { Units.inchesToMeters(651.22 - 182.11), Units.inchesToMeters(158.84)); } } - return null; + return new Translation2d(Units.inchesToMeters(182.11), Units.inchesToMeters(158.84)); + } + + public static Rotation2d getRotationTowardsHub(Translation2d hubpose, Translation2d currentpose) { + double deltaX = hubpose.getX() - currentpose.getX(); + double deltaY = hubpose.getY() - currentpose.getY(); + + double angleRadians = Math.atan2(deltaY, deltaX); + + return new Rotation2d(angleRadians); } // Determines active based on current match time From bba98794ce51b81d00f9c6368e3845d44e4ff54b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 25 Mar 2026 07:01:44 -0400 Subject: [PATCH 07/10] remove test control --- src/main/java/frc/robot/RobotContainer.java | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae3efca..2b6abf1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -119,16 +119,6 @@ private void configureBindings() { controller::getLeftY, controller::getRightX)); - controller - .x() - .toggleOnTrue( - new DrivetrainCommand( - drivetrain, - DrivetrainCommand.Position.AUTO_ALIGN_HUB, - controller::getLeftX, - controller::getLeftY, - controller::getRightX)); - /* Controls */ // Y = Shoot Toggle controller From c15b36ccbcf43cb56e71d3f1d32be56b1b943463 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 25 Mar 2026 12:48:11 -0400 Subject: [PATCH 08/10] Flywheel calculated shot edits --- build.gradle | 2 ++ .../frc/robot/commands/DrivetrainCommand.java | 8 ++--- .../frc/robot/commands/FlywheelCommand.java | 6 ++-- .../frc/robot/helpers/ShotCalculator.java | 31 +++++++++++++++++-- 4 files changed, 39 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 0401926..29cf766 100644 --- a/build.gradle +++ b/build.gradle @@ -74,6 +74,8 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + implementation 'org.apache.commons:commons-math3:3.6.1' } test { diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index 07bd72d..a25b8b3 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -113,7 +113,7 @@ public void execute() { .withRotationalRate( -rightX.getAsDouble() * MaxAngularRate)); // Drive counterclockwise with negative X - System.out.println("TELEOP"); + // System.out.println("TELEOP"); break; // (Needs check) Mode for when shots are from a still position @@ -121,17 +121,17 @@ public void execute() { // make X with wheels, set wheels to brake mode applyRequest.ModuleStates = states; subsystem.setControl(applyRequest); - System.out.println("Drivetrain: Still Shot Configuration"); + // System.out.println("Drivetrain: Still Shot Configuration"); break; // (Incomplete) Mode for moving shots case SOTM: // shoot on the move, reference Mechanical Advantage build log - System.out.println("Drivetrain: SOTM Drive"); + // System.out.println("Drivetrain: SOTM Drive"); break; case AUTO_ALIGN_HUB: - System.out.println("AUTO ALIGN"); + // System.out.println("AUTO ALIGN"); // Recompute the desired heading toward the hub each loop m_targetAngle = ShotCalculator.getRotationTowardsHub( diff --git a/src/main/java/frc/robot/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index f07954e..98e2d8c 100644 --- a/src/main/java/frc/robot/commands/FlywheelCommand.java +++ b/src/main/java/frc/robot/commands/FlywheelCommand.java @@ -59,10 +59,12 @@ public void execute() { // Spins flywheels at estimated speed given distance from hub case CALCULATED_SHOT: isOn = "true"; - subsystem.shoot( ShotCalculator.calculateFlywheelShot(drivetrain.getState().Pose.getTranslation())); - // System.out.println("Flywheel: Calculated Shot"); + System.out.println( + "Flywheel: Calculated Shot" + + ShotCalculator.calculateFlywheelShot( + drivetrain.getState().Pose.getTranslation())); break; // Determines if robot should be prepared to shoot (if during active period or 5 seconds diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index 10085ea..0c44340 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -7,16 +7,25 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.config.FlywheelConfig; import java.util.Optional; +import org.apache.commons.math3.stat.*; +import org.apache.commons.math3.stat.regression.SimpleRegression; public class ShotCalculator { /* Calculation Methods */ + // Calculate variable shot public static double calculateFlywheelShot(Translation2d robotPosition) { // 1. Calculate distance between goal and robot position double distanceToGoal = calculateHubPosition().getDistance(robotPosition); - // 2. Retrieve correlating RPS from map + // 2. Retrieve correlating RPS from map based off of distance + if (distanceToGoal > Units.inchesToMeters(87) || distanceToGoal < Units.inchesToMeters(43)) { + System.out.println(getRPM(distanceToGoal)); + return getRPM(distanceToGoal); + } + System.out.println(DISTANCE_TO_FLYWHEEL.get(distanceToGoal)); return DISTANCE_TO_FLYWHEEL.get(distanceToGoal); } + // Determine if flywheel should be running before active period and run at mid level speed public static double calculateFlywheelDefaultShot(Translation2d robotPosition) { // 1. Determine if within shooter run period (five seconds before or during active period) if (shouldRunShooter(5)) { @@ -144,8 +153,26 @@ public static boolean shouldRunShooter(double graceSeconds) { DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(141), 70.0); DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(90), 60.0); DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(61), 50.0); - DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(43), 40.0); + DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(43), 47.0); DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(74), 50.0); DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(134), 65.0); } + + // Distance (m) -> Speed (RPS) + private static final SimpleRegression regression = new SimpleRegression(); + + // Set inputs + static { + regression.addData(Units.inchesToMeters(174), 87.0); + regression.addData(Units.inchesToMeters(141), 70.0); + regression.addData(Units.inchesToMeters(90), 60.0); + regression.addData(Units.inchesToMeters(61), 50.0); + regression.addData(Units.inchesToMeters(43), 47.0); + regression.addData(Units.inchesToMeters(74), 50.0); + regression.addData(Units.inchesToMeters(134), 65.0); + } + + public static double getRPM(double distance) { + return regression.getSlope() * distance + regression.getIntercept(); + } } From 373848e02e03aed467cd26a3578daa9dc261f864 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 25 Mar 2026 15:53:11 -0400 Subject: [PATCH 09/10] Improved subsystem state logging --- src/main/java/frc/robot/Robot.java | 13 +++++--- .../frc/robot/commands/DrivetrainCommand.java | 10 +++--- .../frc/robot/commands/FlywheelCommand.java | 25 +++++---------- .../frc/robot/commands/HopperCommand.java | 8 +---- .../frc/robot/commands/IntakeCommand.java | 2 -- .../frc/robot/commands/KickerCommand.java | 4 +++ .../java/frc/robot/subsystems/Hopper.java | 32 +++++++++++-------- .../java/frc/robot/subsystems/Intake.java | 10 ++++++ 8 files changed, 55 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 04920b1..d3001a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,6 @@ package frc.robot; -import static frc.robot.RobotContainer.shooterState; - import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Pose2d; @@ -19,9 +17,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.DrivetrainCommand; import frc.robot.commands.FlywheelCommand; +import frc.robot.commands.KickerCommand; import frc.robot.config.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Hopper; +import frc.robot.subsystems.Intake; @Logged public class Robot extends TimedRobot { @@ -48,8 +50,11 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - SmartDashboard.putString("shoot-state", shooterState); - SmartDashboard.putString("shoot-on", FlywheelCommand.isOn); + SmartDashboard.putString("Flywheel State", FlywheelCommand.flywheelState); + SmartDashboard.putString("Drivetrain State", DrivetrainCommand.drivetrainState); + SmartDashboard.putString("Hopper State", Hopper.hopperState); + SmartDashboard.putString("Intake State", Intake.intakeState); + SmartDashboard.putString("Kicker State", KickerCommand.kickerState); try { var pose = drivetrain.getState().Pose; SmartDashboard.putNumber("OdometryX", pose.getX()); diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index a25b8b3..a2ac544 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -31,6 +31,7 @@ public static enum Position { private DoubleSupplier leftY; private DoubleSupplier leftX; private DoubleSupplier rightX; + public static String drivetrainState = "Stopped"; private final double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -113,7 +114,7 @@ public void execute() { .withRotationalRate( -rightX.getAsDouble() * MaxAngularRate)); // Drive counterclockwise with negative X - // System.out.println("TELEOP"); + drivetrainState = "Teleop Drive"; break; // (Needs check) Mode for when shots are from a still position @@ -121,17 +122,16 @@ public void execute() { // make X with wheels, set wheels to brake mode applyRequest.ModuleStates = states; subsystem.setControl(applyRequest); - // System.out.println("Drivetrain: Still Shot Configuration"); + drivetrainState = "Still Shot"; break; // (Incomplete) Mode for moving shots case SOTM: // shoot on the move, reference Mechanical Advantage build log - // System.out.println("Drivetrain: SOTM Drive"); + drivetrainState = "Shoot on the Move Drive"; break; case AUTO_ALIGN_HUB: - // System.out.println("AUTO ALIGN"); // Recompute the desired heading toward the hub each loop m_targetAngle = ShotCalculator.getRotationTowardsHub( @@ -155,7 +155,7 @@ public void execute() { subsystem.setControl( drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput)); } - + drivetrainState = "Auto Align"; break; default: diff --git a/src/main/java/frc/robot/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index 98e2d8c..3198ba2 100644 --- a/src/main/java/frc/robot/commands/FlywheelCommand.java +++ b/src/main/java/frc/robot/commands/FlywheelCommand.java @@ -21,7 +21,7 @@ public static enum Position { private Flywheel subsystem; private Position pose; private CommandSwerveDrivetrain drivetrain; - public static String isOn = ""; + public static String flywheelState = "Stopped"; public FlywheelCommand(Flywheel subsystem, Position pose, CommandSwerveDrivetrain drivetrain) { this.pose = pose; @@ -37,34 +37,25 @@ public void execute() { // Spins flywheels at proper speed for shooting from hub case HUB_SHOT: subsystem.shoot(FlywheelConfig.FLYWHEEL_HUB_SHOT_SPEED); - isOn = "true"; - // System.out.println("Flywheel: Hub Shot"); + flywheelState = "Hub Shot"; break; // Spins flywheels at proper speed for shooting from tower case TOWER_SHOT: subsystem.shoot(FlywheelConfig.FLYWHEEL_TOWER_SHOT_SPEED); - isOn = "true"; - - // System.out.println("Flywheel: Tower Shot"); + flywheelState = "Tower Shot"; break; case TRENCH_SHOT: subsystem.shoot(FlywheelConfig.FLYWHEEL_TRENCH_SHOT_SPEED); - isOn = "true"; - - // System.out.println("Flywheel: Trench Shot"); + flywheelState = "Trench Shot"; break; // Spins flywheels at estimated speed given distance from hub case CALCULATED_SHOT: - isOn = "true"; + flywheelState = "Calculated Shot"; subsystem.shoot( ShotCalculator.calculateFlywheelShot(drivetrain.getState().Pose.getTranslation())); - System.out.println( - "Flywheel: Calculated Shot" - + ShotCalculator.calculateFlywheelShot( - drivetrain.getState().Pose.getTranslation())); break; // Determines if robot should be prepared to shoot (if during active period or 5 seconds @@ -74,13 +65,13 @@ public void execute() { subsystem.shoot( ShotCalculator.calculateFlywheelDefaultShot( drivetrain.getState().Pose.getTranslation())); - // System.out.println("Flywheel: Default Shot"); + flywheelState = "Default Shot"; break; // Runs flywheels in outtake direction in case of jam case OUTTAKE: subsystem.outtake(FlywheelConfig.FLYWHEEL_OUTTAKE_SPEED); - // System.out.println("Flywheel: Outtake"); + flywheelState = "Outtake"; break; default: @@ -91,7 +82,7 @@ public void execute() { @Override public void end(boolean interrupted) { subsystem.stop(); - isOn = "false"; + flywheelState = "none"; } @Override diff --git a/src/main/java/frc/robot/commands/HopperCommand.java b/src/main/java/frc/robot/commands/HopperCommand.java index 3fc74aa..932dbae 100644 --- a/src/main/java/frc/robot/commands/HopperCommand.java +++ b/src/main/java/frc/robot/commands/HopperCommand.java @@ -34,27 +34,21 @@ public void initialize() { // extend case EXTEND_RETRACT: if (isExtended) { - System.out.println("Hopper: Retracting"); subsystem.move(HopperConfig.HOPPER_RETRACT_ROTATION); RobotContainer.isExtended = false; - System.out.println(RobotContainer.isExtended); } else { - System.out.println("Hopper: Extending"); subsystem.move(HopperConfig.HOPPER_EXTEND_ROTATION); RobotContainer.isExtended = true; - System.out.println(RobotContainer.isExtended); } break; - // (Needs check) Retracts then extends hopper back to original position to push balls into + // Retracts then extends hopper back to original position to push balls into // kicker for shooting case SHOOT_RETRACT: - // System.out.println("Hopper: Shoot Retracting"); subsystem.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION); break; case SHOOT_EXTEND: - // System.out.println("Hopper: Shoot Extending"); subsystem.move(HopperConfig.HOPPER_EXTEND_ROTATION); break; diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index dcfb03e..a784b01 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -29,7 +29,6 @@ public void initialize() { // Runs intake case INTAKE: subsystem.intake(IntakeConfig.INTAKE_INTAKE_SPEED); - // System.out.println("Intake: Intaking"); break; // Runs intake in outtaking direction @@ -40,7 +39,6 @@ public void initialize() { // Slow intake for shooting case SLOW_INTAKE: subsystem.intake(IntakeConfig.INTAKE_SLOW_SPEED); - // System.out.println("Intake: run for shooting"); break; default: diff --git a/src/main/java/frc/robot/commands/KickerCommand.java b/src/main/java/frc/robot/commands/KickerCommand.java index 7203616..4d760d3 100644 --- a/src/main/java/frc/robot/commands/KickerCommand.java +++ b/src/main/java/frc/robot/commands/KickerCommand.java @@ -14,6 +14,7 @@ public static enum Position { private Kicker subsystem; private Position pose; + public static String kickerState = "Stopped"; public KickerCommand(Kicker subsystem, Position pose) { this.pose = pose; @@ -28,11 +29,13 @@ public void execute() { // Intakes balls to shooter case INTAKE: subsystem.intake(KickerConfig.KICKER_INTAKE_SPEED); + kickerState = "Intaking"; break; // Pushes balls out of shooter area to hopper area case OUTTAKE: subsystem.outtake(KickerConfig.KICKER_OUTTAKE_SPEED); + kickerState = "Outtaking"; break; default: @@ -43,6 +46,7 @@ public void execute() { @Override public void end(boolean interrupted) { subsystem.stop(); + kickerState = "Stopped"; } @Override diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index bd07a1a..37b7860 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -14,6 +14,7 @@ @Logged public class Hopper extends SubsystemBase { protected TalonFX hopper; + public static String hopperState = "None"; public Hopper() { @@ -52,25 +53,41 @@ public Hopper() { public void move(double rotation) { hopper.setControl(new MotionMagicVoltage(rotation).withSlot(0)); + if (rotation == HopperConfig.HOPPER_EXTEND_ROTATION) { + hopperState = "Move: Extending"; + } else if (rotation == HopperConfig.HOPPER_RETRACT_ROTATION) { + hopperState = "Move: Retracting"; + } else { + hopperState = "Move: Unknown"; + } } public void slowMove(double rotation) { hopper.setControl(new MotionMagicVoltage(rotation).withSlot(1)); + if (rotation == HopperConfig.HOPPER_EXTEND_ROTATION) { + hopperState = "Slow Move: Extending"; + } else if (rotation == HopperConfig.HOPPER_RETRACT_ROTATION) { + hopperState = "Slow Move: Retracting"; + } else { + hopperState = "Slow Move: Unknown"; + } } public void extendDirectional(double speed) { speed = Math.abs(speed); hopper.setControl(new VoltageOut(-speed)); + hopperState = "Extend Directional"; } public void retractDirectional(double speed) { speed = Math.abs(speed); hopper.setControl(new VoltageOut(-speed)); + hopperState = "Retract Directional"; } public void stop() { hopper.stopMotor(); - System.out.println("Hopper: Stopped"); + hopperState = "Stopped"; } public void zero() { @@ -109,23 +126,10 @@ public static double getRotation(boolean isExtended) { public void toggleExtend() { if (extended) { move(HopperConfig.HOPPER_RETRACT_ROTATION); - System.out.println("Hopper: retracting"); extended = false; } else { move(HopperConfig.HOPPER_EXTEND_ROTATION); - System.out.println("Hopper: extending"); extended = true; } } - - // @Override - // public void periodic() { - // if (DriverStation.isDisabled()) { - // hopper.setNeutralMode(NeutralModeValue.Coast); - // } else if (isRetractedByPosition()) { - // hopper.setNeutralMode(NeutralModeValue.Brake); - // } else { - // hopper.setNeutralMode(NeutralModeValue.Coast); - // } - // } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2a08466..293c6fb 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,6 +13,7 @@ @Logged public class Intake extends SubsystemBase { protected TalonFX intake; + public static String intakeState = "Stopped"; public Intake() { intake = new TalonFX(CANMappings.INTAKE_MOTOR_ID); @@ -34,14 +35,23 @@ public Intake() { public void intake(double speed) { speed = Math.abs(speed); intake.setControl(new VoltageOut(speed)); + if (speed == IntakeConfig.INTAKE_INTAKE_SPEED) { + intakeState = "Intaking: Default"; + } else if (speed == IntakeConfig.INTAKE_SLOW_SPEED) { + intakeState = "Intaking: Slow"; + } else { + intakeState = "Intaking: Unknown"; + } } public void outtake(double speed) { speed = Math.abs(speed); intake.setControl(new VoltageOut(-speed)); + intakeState = "Outtaking"; } public void stop() { intake.stopMotor(); + intakeState = "Stopped"; } } From ded0bdf54c8a2d0fb8598ae4ca550f7fa3fc4943 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 25 Mar 2026 16:08:30 -0400 Subject: [PATCH 10/10] Add REVLib --- vendordeps/REVLib.json | 133 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 vendordeps/REVLib.json diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..c6b1b9e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,133 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.5", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.5" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.5", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.5", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.5", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.5", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file