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 @@ + 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/Robot.java b/src/main/java/frc/robot/Robot.java index c226996..d3001a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,31 +4,41 @@ 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.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; +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.DrivetrainCommand; import frc.robot.commands.FlywheelCommand; +import frc.robot.commands.KickerCommand; import frc.robot.config.TunerConstants; -import frc.robot.config.VisionConfig; import frc.robot.subsystems.CommandSwerveDrivetrain; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; +import frc.robot.subsystems.Hopper; +import frc.robot.subsystems.Intake; @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 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,21 +50,22 @@ 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("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()); + SmartDashboard.putNumber("OdometryY", pose.getY()); + SmartDashboard.putNumber("OdometryRotDeg", pose.getRotation().getDegrees()); + SmartDashboard.putNumber("SimVisionLastInject", m_lastSimVisionTime); + } catch (Exception ignored) { } - SmartDashboard.putString("shoot-state", shooterState); - SmartDashboard.putString("shoot-on", FlywheelCommand.isOn); + robotPose = drivetrain.getState().Pose; + publisher.set(robotPose); + arrayPublisher.set(new Pose2d[] {robotPose}); } @Override @@ -109,5 +120,24 @@ 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); + 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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 64a51bd..2b6abf1 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; @@ -28,11 +27,9 @@ 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); - private SwerveDriveState driveState = new SwerveDriveState(); private final SendableChooser autoChooser; public static boolean isExtended = false; public static String shooterState = "none"; @@ -176,8 +173,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/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/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index c59c695..a2ac544 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; @@ -26,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 @@ -48,6 +54,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 +68,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 +114,7 @@ public void execute() { .withRotationalRate( -rightX.getAsDouble() * MaxAngularRate)); // Drive counterclockwise with negative X + drivetrainState = "Teleop Drive"; break; // (Needs check) Mode for when shots are from a still position @@ -85,13 +122,40 @@ 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: + // 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)); + } + drivetrainState = "Auto Align"; break; default: @@ -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/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index f07954e..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,32 +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"); break; // Determines if robot should be prepared to shoot (if during active period or 5 seconds @@ -72,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: @@ -89,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/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/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/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index adae354..0c44340 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -1,21 +1,31 @@ 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; 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 = calculateGoalPosition().getDistance(robotPosition); - // 2. Retrieve correlating RPS from map + double distanceToGoal = calculateHubPosition().getDistance(robotPosition); + // 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)) { @@ -27,7 +37,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 +47,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 @@ -134,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(); + } } 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); - // } -} 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() { 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"; } } 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); } 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