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/.vscode/settings.json b/.vscode/settings.json index 9ce64ca..418b64c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.dependency.enableDependencyCheckup": false, - "java.debug.settings.onBuildFailureProceed": true + "java.debug.settings.onBuildFailureProceed": true, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } 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/deploy/pathplanner/autos/center to back outpost side.auto b/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/center to back outpost side.auto rename to src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto index 8be7e39..633ebd4 100644 --- a/src/main/deploy/pathplanner/autos/center to back outpost side.auto +++ b/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "hopper deploy" - } - }, { "type": "named", "data": { @@ -17,19 +11,32 @@ } }, { - "type": "deadline", + "type": "parallel", "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "1 Outpost side center" + "commands": [ + { + "type": "path", + "data": { + "pathName": "1 Depot side center" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] } }, { "type": "named", "data": { - "name": "intake" + "name": "hopper deploy" } } ] @@ -38,7 +45,7 @@ { "type": "path", "data": { - "pathName": "2 center to trench outpost" + "pathName": "2 Center back depot" } }, { @@ -75,7 +82,7 @@ { "type": "path", "data": { - "pathName": "3 to middle outpost" + "pathName": "3 to middle depot" } } ] diff --git a/src/main/deploy/pathplanner/autos/center to back depot side.auto b/src/main/deploy/pathplanner/autos/center to back depot side.auto index 096269d..c136d35 100644 --- a/src/main/deploy/pathplanner/autos/center to back depot side.auto +++ b/src/main/deploy/pathplanner/autos/center to back depot side.auto @@ -11,25 +11,45 @@ } }, { - "type": "deadline", + "type": "parallel", "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "1 Depot side center" + "commands": [ + { + "type": "path", + "data": { + "pathName": "1 Depot side center" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] } }, { - "type": "named", - "data": { - "name": "hopper deploy" - } - }, - { - "type": "named", + "type": "sequential", "data": { - "name": "intake" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "hopper deploy" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/1 Depot side center.path b/src/main/deploy/pathplanner/paths/1 Depot side center.path index 48b3073..d07f9ee 100644 --- a/src/main/deploy/pathplanner/paths/1 Depot side center.path +++ b/src/main/deploy/pathplanner/paths/1 Depot side center.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.74248120300752, - "y": 4.862545112781954 + "x": 7.753707692307692, + "y": 5.793184615384616 }, "prevControl": { - "x": 7.636977443609023, - "y": 8.001281954887219 + "x": 7.802546153846153, + "y": 7.760676923076924 }, "nextControl": null, "isLocked": false, @@ -34,21 +34,7 @@ "rotationDegrees": -58.149754025069136 } ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6399186643835615, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/2 Center back depot.path b/src/main/deploy/pathplanner/paths/2 Center back depot.path index 07bdc61..f146a1c 100644 --- a/src/main/deploy/pathplanner/paths/2 Center back depot.path +++ b/src/main/deploy/pathplanner/paths/2 Center back depot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.735228859581072, - "y": 4.791419705197828 + "x": 7.649053846153848, + "y": 5.96063076923077 }, "prevControl": null, "nextControl": { - "x": 8.002614429790537, - "y": 7.880426687354538 + "x": 7.537423076923078, + "y": 7.593230769230768 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/3 outpost double dip out.path b/src/main/deploy/pathplanner/paths/3 outpost double dip out.path new file mode 100644 index 0000000..2674c79 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 outpost double dip out.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.763007521856398, + "y": 0.5450873093377981 + }, + "prevControl": null, + "nextControl": { + "x": 6.79049072265625, + "y": 0.5080256580171136 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.756997593470982, + "y": 3.5701235235305067 + }, + "prevControl": { + "x": 6.823525041852679, + "y": 1.1877271670386897 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6108732876712327, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 89.52460411845328 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 79.34104871052683 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 outpost double dip back.path b/src/main/deploy/pathplanner/paths/4 outpost double dip back.path new file mode 100644 index 0000000..4c004d8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 outpost double dip back.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.581018473307291, + "y": 3.4284531947544643 + }, + "prevControl": null, + "nextControl": { + "x": 6.592998511904762, + "y": 1.1323131161644344 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.681951090494791, + "y": 0.5692513020833336 + }, + "prevControl": { + "x": 6.184096854073661, + "y": 0.4140205891927087 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 77.58672595112199 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 89.64828909075314 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c226996..93db8a3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,34 +4,36 @@ 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.StructPublisher; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; 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.config.TunerConstants; -import frc.robot.config.VisionConfig; +import frc.robot.commands.KickerCommand; 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(); - + private Pose2d robotPose = new Pose2d(); private final RobotContainer m_robotContainer; + private final CommandSwerveDrivetrain drivetrain; + StructPublisher publisher = + NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish(); public Robot() { m_robotContainer = new RobotContainer(); + drivetrain = m_robotContainer.getDrivetrain(); Epilogue.bind(this); DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); @@ -40,21 +42,17 @@ 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); + 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); + SmartDashboard.putBoolean( + "Flywheel On", + !(FlywheelCommand.flywheelState.equals("none") + || FlywheelCommand.flywheelState.equals("Stopped"))); + robotPose = drivetrain.getState().Pose; + publisher.set(robotPose); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 64a51bd..ca31d9c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,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; @@ -17,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.*; -import frc.robot.config.CANMappings; import frc.robot.config.HopperConfig; import frc.robot.config.IntakeConfig; import frc.robot.config.TunerConstants; @@ -28,14 +25,11 @@ 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"; private Command shootGroup = Commands.parallel(new KickerCommand(kicker, KickerCommand.Position.INTAKE)) @@ -78,12 +72,18 @@ public RobotContainer() { NamedCommands.registerCommand( "hopper retract", - Commands.runEnd( - () -> hopper.move(HopperConfig.HOPPER_RETRACT_ROTATION), - () -> hopper.stop(), - hopper) - .alongWith(Commands.run(() -> System.out.println("Hopper Retracted"))) - .withDeadline(Commands.waitSeconds(2))); + Commands.repeatingSequence( + Commands.runEnd( + () -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION), + () -> hopper.stop(), + hopper) + .withDeadline(Commands.waitSeconds(.75)), + Commands.runEnd( + () -> hopper.slowMove(HopperConfig.HOPPER_EXTEND_ROTATION), + () -> hopper.stop(), + hopper) + .withDeadline(Commands.waitSeconds(.75))) + .withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "intake", @@ -103,6 +103,28 @@ public RobotContainer() { new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain)) .withDeadline(Commands.waitSeconds(8))); + NamedCommands.registerCommand( + "short shoot", + Commands.parallel( + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain)) + .withDeadline(Commands.waitSeconds(3))); + + NamedCommands.registerCommand( + "hopper down", + Commands.runEnd( + () -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION), () -> hopper.stop(), hopper) + .withDeadline(Commands.waitSeconds(0.5))); + + NamedCommands.registerCommand( + "hub shot", + Commands.parallel( + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain)) + .withDeadline(Commands.waitSeconds(10))); + autoChooser = AutoBuilder.buildAutoChooser("Test"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -123,6 +145,15 @@ private void configureBindings() { controller::getRightX)); /* Controls */ + controller + .rightTrigger() + .toggleOnTrue( + new DrivetrainCommand( + drivetrain, + DrivetrainCommand.Position.AUTO_ALIGN_HUB, + controller::getLeftX, + controller::getLeftY, + controller::getRightX)); // Y = Shoot Toggle controller .y() @@ -155,38 +186,43 @@ private void configureBindings() { .withDeadline(Commands.waitSeconds(2)) .andThen(Commands.runOnce(() -> isExtended = !isExtended))); // Back button = Zero Pigeon - controller.back().onTrue(Commands.runOnce(() -> zeroPigeon())); + controller.back().onTrue(Commands.runOnce(drivetrain::seedFieldCentric)); // Right Bumper = Flywheel Toggle for hub shot speeds controller .rightBumper() - .toggleOnTrue( - new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain) - .alongWith(Commands.run(() -> shooterState = "HUB"))); - // Left Bumper = Flywheel Toggle for tower shot speeds + .toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain)); + // Left Bumper = Flywheel Toggle for trench shot speeds controller .leftBumper() .toggleOnTrue( - new FlywheelCommand(flywheel, FlywheelCommand.Position.TOWER_SHOT, drivetrain) - .alongWith(Commands.run(() -> shooterState = "TOWER"))); - // Left Trigger = Flywheel Toggle for trench shot speeds + new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain)); + // Left Trigger = Flywheel Toggle for calculated shot speeds controller .leftTrigger() .toggleOnTrue( - new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain) - .alongWith(Commands.run(() -> shooterState = "TRENCH"))); + new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain)); // 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)); + // B = Shoot on the move toggle - placeholder button + controller + .b() + .toggleOnTrue( + Commands.parallel( + new DrivetrainCommand( + drivetrain, + DrivetrainCommand.Position.SOTM, + controller::getLeftX, + controller::getLeftY, + controller::getRightX), + new FlywheelCommand( + flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain))); } public Command getAutonomousCommand() { return autoChooser.getSelected(); } - public static void zeroPigeon() { - Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); - pigeon.reset(); - System.out.println("Reset Pigeon"); + public CommandSwerveDrivetrain getDrivetrain() { + return drivetrain; } } 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..e1ac82c 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -5,11 +5,16 @@ 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.ChassisSpeeds; 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 +23,8 @@ public class DrivetrainCommand extends Command { public static enum Position { TELEOP, STILL_SHOT, - SOTM + SOTM, + AUTO_ALIGN_HUB } private CommandSwerveDrivetrain subsystem; @@ -26,6 +32,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 +55,9 @@ 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; + private static final double FUEL_EXIT_VELOCITY = 10.0; // This needs to be tuned public DrivetrainCommand( CommandSwerveDrivetrain subsystem, @@ -60,10 +70,38 @@ public DrivetrainCommand( this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; + double alignP = 75; + double alignI = 0; + double alignD = 10; + 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 +116,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 +124,63 @@ 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"; + double vx = -leftY.getAsDouble() * MaxSpeed; + double vy = -leftX.getAsDouble() * MaxSpeed; + Translation2d robotPosition = subsystem.getState().Pose.getTranslation(); + ChassisSpeeds speeds = subsystem.getState().Speeds; + Translation2d robotVelocity = + new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + Translation2d goal = ShotCalculator.calculateHubPosition(); + double distance = robotPosition.getDistance(goal); + double flightTime = distance / FUEL_EXIT_VELOCITY; + Translation2d toGoal = goal.minus(robotPosition); + Translation2d toGoalDir = + toGoal.getNorm() > 1e-6 ? toGoal.div(toGoal.getNorm()) : new Translation2d(); + Translation2d lateralVelocity = + robotVelocity.minus(toGoalDir.times(robotVelocity.dot(toGoalDir))); + Translation2d virtualGoal = goal.minus(lateralVelocity.times(flightTime)); + Rotation2d targetAngle = virtualGoal.minus(robotPosition).getAngle(); + double currentHeading = subsystem.getState().Pose.getRotation().getRadians(); + double targetHeading = targetAngle.getRadians(); + double error = targetHeading - currentHeading; + error = Math.atan2(Math.sin(error), Math.cos(error)); + double kP = 50; // tune + double omega = error * kP; + omega = Math.max(-MaxAngularRate, Math.min(MaxAngularRate, omega)); + subsystem.setControl(drive.withVelocityX(vx).withVelocityY(vy).withRotationalRate(omega)); + + 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 error1 to [-pi, pi] + double error1 = + Math.atan2(Math.sin(targetRad - currentRad), Math.cos(targetRad - currentRad)); + double absError = Math.abs(error1); + double angleTolerance = Math.toRadians(0.5); // 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 + subsystem.setControl( + drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput)); + } + drivetrainState = "Auto Align"; break; default: @@ -104,6 +193,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/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index 56a0b26..5659ca6 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -8,8 +8,6 @@ public class CANMappings { public static final int INTAKE_MOTOR_ID = 3; - public static final int CLIMB_MOTOR_ID = 22; // unset - public static final int FLYWHEEL_RIGHT_MOTOR_ID = 19; public static final int FLYWHEEL_LEFT_MOTOR_ID = 2; 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/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index ba818ba..834d2fc 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -1,22 +1,22 @@ package frc.robot.config; public class FlywheelConfig { - public static final double FLYWHEEL_RIGHT_SUPPLY_CURRENT_LIMIT = 70; - public static final double FLYWHEEL_RIGHT_STATOR_CURRENT_LIMIT = 80; + public static final double FLYWHEEL_RIGHT_SUPPLY_CURRENT_LIMIT = 50; + public static final double FLYWHEEL_RIGHT_STATOR_CURRENT_LIMIT = 60; public static final double FLYWHEEL_RIGHT_GEAR_RATIO = 1; - public static final double FLYWHEEL_LEFT_SUPPLY_CURRENT_LIMIT = 70; - public static final double FLYWHEEL_LEFT_STATOR_CURRENT_LIMIT = 80; + public static final double FLYWHEEL_LEFT_SUPPLY_CURRENT_LIMIT = 50; + public static final double FLYWHEEL_LEFT_STATOR_CURRENT_LIMIT = 60; public static final double FLYWHEEL_LEFT_GEAR_RATIO = 1; // Set public static final double FLYWHEEL_RIGHT_MAX_CRUISE_VELOCITY = 3000; public static final double FLYWHEEL_RIGHT_TARGET_ACCELERATION = 500; - public static final double FLYWHEEL_RIGHT_P = 10; + public static final double FLYWHEEL_RIGHT_P = 0.5; public static final double FLYWHEEL_RIGHT_I = 0; public static final double FLYWHEEL_RIGHT_D = 0; - public static final double FLYWHEEL_RIGHT_S = 0; - public static final double FLYWHEEL_RIGHT_V = 0; + public static final double FLYWHEEL_RIGHT_S = 0.18; + public static final double FLYWHEEL_RIGHT_V = 0.115; public static final double FLYWHEEL_RIGHT_A = 0; // Set @@ -30,9 +30,9 @@ public class FlywheelConfig { public static final double FLYWHEEL_LEFT_A = 0; // Set - public static final double FLYWHEEL_HUB_SHOT_SPEED = 47; + public static final double FLYWHEEL_HUB_SHOT_SPEED = 43; public static final double FLYWHEEL_TOWER_SHOT_SPEED = 65; - public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 65; + public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 67; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40; diff --git a/src/main/java/frc/robot/config/HopperConfig.java b/src/main/java/frc/robot/config/HopperConfig.java index a264118..2a1e41f 100644 --- a/src/main/java/frc/robot/config/HopperConfig.java +++ b/src/main/java/frc/robot/config/HopperConfig.java @@ -8,21 +8,21 @@ public class HopperConfig { // Set public static final double HOPPER_MAX_CRUISE_VELOCITY = 500; public static final double HOPPER_TARGET_ACCELERATION = 500; - public static final double HOPPER_P = 1.25; + public static final double HOPPER_P = 1; public static final double HOPPER_I = 0; public static final double HOPPER_D = 0; public static final double HOPPER_S = 0; public static final double HOPPER_V = 0; public static final double HOPPER_A = 0; - public static final double SLOW_HOPPER_P = 1.25; + public static final double SLOW_HOPPER_P = 0.5; public static final double SLOW_HOPPER_I = 0; public static final double SLOW_HOPPER_D = 0; public static final double HOPPER_TOLERANCE = 0.2; // Set - public static final double HOPPER_EXTEND_ROTATION = 4.1; - public static final double HOPPER_RETRACT_ROTATION = 0.1; + public static final double HOPPER_EXTEND_ROTATION = -17; + public static final double HOPPER_RETRACT_ROTATION = -0.1; public static final double HOPPER_RETRACT_FOR_NEUTRAL_MODE_ROTATION = 1.752; } diff --git a/src/main/java/frc/robot/config/IntakeConfig.java b/src/main/java/frc/robot/config/IntakeConfig.java index 7ace021..ad313c6 100644 --- a/src/main/java/frc/robot/config/IntakeConfig.java +++ b/src/main/java/frc/robot/config/IntakeConfig.java @@ -1,12 +1,12 @@ package frc.robot.config; public class IntakeConfig { - public static final double INTAKE_STATOR_CURRENT_LIMIT = 80; - public static final double INTAKE_SUPPLY_CURRENT_LIMIT = 70; + public static final double INTAKE_STATOR_CURRENT_LIMIT = 40; + public static final double INTAKE_SUPPLY_CURRENT_LIMIT = 35; public static final double INTAKE_GEAR_RATIO = 1; // Set - public static final double INTAKE_INTAKE_SPEED = 4; + public static final double INTAKE_INTAKE_SPEED = 10; public static final double INTAKE_OUTTAKE_SPEED = 4; public static final double INTAKE_SLOW_SPEED = 3; diff --git a/src/main/java/frc/robot/config/KickerConfig.java b/src/main/java/frc/robot/config/KickerConfig.java index 2c229fb..470957f 100644 --- a/src/main/java/frc/robot/config/KickerConfig.java +++ b/src/main/java/frc/robot/config/KickerConfig.java @@ -1,11 +1,11 @@ package frc.robot.config; public class KickerConfig { - public static final double KICKER_FRONT_STATOR_CURRENT_LIMIT = 80; - public static final double KICKER_FRONT_SUPPLY_CURRENT_LIMIT = 70; + public static final double KICKER_FRONT_STATOR_CURRENT_LIMIT = 60; + public static final double KICKER_FRONT_SUPPLY_CURRENT_LIMIT = 50; public static final double KICKER_FRONT_GEAR_RATIO = 3; - public static final double KICKER_BACK_STATOR_CURRENT_LIMIT = 80; - public static final double KICKER_BACK_SUPPLY_CURRENT_LIMIT = 70; + public static final double KICKER_BACK_STATOR_CURRENT_LIMIT = 60; + public static final double KICKER_BACK_SUPPLY_CURRENT_LIMIT = 50; public static final double KICKER_BACK_GEAR_RATIO = 3; // Set diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 83f3722..3c37a44 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -55,7 +55,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(60); // from 120 + private static final Current kSlipCurrent = Amps.of(120); // from 120 // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. 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/LimelightHelpers.java b/src/main/java/frc/robot/helpers/LimelightHelpers.java deleted file mode 100644 index 7ef139c..0000000 --- a/src/main/java/frc/robot/helpers/LimelightHelpers.java +++ /dev/null @@ -1,1691 +0,0 @@ -// LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) - -package frc.robot.helpers; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.DoubleArrayEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.Map; -import java.util.concurrent.CompletableFuture; -import java.util.concurrent.ConcurrentHashMap; - -/** - * LimelightHelpers provides static methods and classes for interfacing with Limelight vision - * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural - * Networks, and standard color/retroreflective tracking. - */ -public class LimelightHelpers { - - private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - - /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() { - return toPose3D(cameraPose_TargetSpace); - } - - public Pose3d getRobotPose_FieldSpace() { - return toPose3D(robotPose_FieldSpace); - } - - public Pose3d getRobotPose_TargetSpace() { - return toPose3D(robotPose_TargetSpace); - } - - public Pose3d getTargetPose_CameraSpace() { - return toPose3D(targetPose_CameraSpace); - } - - public Pose3d getTargetPose_RobotSpace() { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() { - return toPose2D(cameraPose_TargetSpace); - } - - public Pose2d getRobotPose_FieldSpace2D() { - return toPose2D(robotPose_FieldSpace); - } - - public Pose2d getRobotPose_TargetSpace2D() { - return toPose2D(robotPose_TargetSpace); - } - - public Pose2d getTargetPose_CameraSpace2D() { - return toPose2D(targetPose_CameraSpace); - } - - public Pose2d getTargetPose_RobotSpace2D() { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** Represents a Barcode Target Result extracted from JSON Output */ - public static class LimelightTarget_Barcode { - - /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ - @JsonProperty("fam") - public String family; - - /** Gets the decoded data content of the barcode */ - @JsonProperty("data") - public String data; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - - public LimelightTarget_Barcode() {} - - public String getFamily() { - return family; - } - } - - /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() {} - } - - /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - public LimelightTarget_Detector() {} - } - - /** Limelight Results object, parsed from a Limelight's JSON results output. */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public LimelightResults() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - } - } - - /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ - public static class RawFiducial { - public int id = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double distToCamera = 0; - public double distToRobot = 0; - public double ambiguity = 0; - - public RawFiducial( - int id, - double txnc, - double tync, - double ta, - double distToCamera, - double distToRobot, - double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ - public static class RawDetection { - public int classId = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double corner0_X = 0; - public double corner0_Y = 0; - public double corner1_X = 0; - public double corner1_Y = 0; - public double corner2_X = 0; - public double corner2_Y = 0; - public double corner3_X = 0; - public double corner3_Y = 0; - - public RawDetection( - int classId, - double txnc, - double tync, - double ta, - double corner0_X, - double corner0_Y, - double corner1_X, - double corner1_Y, - double corner2_X, - double corner2_Y, - double corner3_X, - double corner3_Y) { - this.classId = classId; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.corner0_X = corner0_X; - this.corner0_Y = corner0_Y; - this.corner1_X = corner1_X; - this.corner1_Y = corner1_Y; - this.corner2_X = corner2_X; - this.corner2_Y = corner2_Y; - this.corner3_X = corner3_X; - this.corner3_Y = corner3_Y; - } - } - - /** Represents a 3D Pose Estimate. */ - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - - public RawFiducial[] rawFiducials; - public boolean isMegaTag2; - - /** Instantiates a PoseEstimate object with default values */ - public PoseEstimate() { - this.pose = new Pose2d(); - this.timestampSeconds = 0; - this.latency = 0; - this.tagCount = 0; - this.tagSpan = 0; - this.avgTagDist = 0; - this.avgTagArea = 0; - this.rawFiducials = new RawFiducial[] {}; - this.isMegaTag2 = false; - } - - public PoseEstimate( - Pose2d pose, - double timestampSeconds, - double latency, - int tagCount, - double tagSpan, - double avgTagDist, - double avgTagArea, - RawFiducial[] rawFiducials, - boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - } - - /** Encapsulates the state of an internal Limelight IMU. */ - public static class IMUData { - public double robotYaw = 0.0; - public double Roll = 0.0; - public double Pitch = 0.0; - public double Yaw = 0.0; - public double gyroX = 0.0; - public double gyroY = 0.0; - public double gyroZ = 0.0; - public double accelX = 0.0; - public double accelY = 0.0; - public double accelZ = 0.0; - - public IMUData() {} - - public IMUData(double[] imuData) { - if (imuData != null && imuData.length >= 10) { - this.robotYaw = imuData[0]; - this.Roll = imuData[1]; - this.Pitch = imuData[2]; - this.Yaw = imuData[3]; - this.gyroX = imuData[4]; - this.gyroY = imuData[5]; - this.gyroZ = imuData[6]; - this.accelX = imuData[7]; - this.accelY = imuData[8]; - this.accelZ = imuData[9]; - } - } - } - - private static ObjectMapper mapper; - - /** Print JSON Parse time to the console in milliseconds */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z, - * roll, pitch, yaw] where angles are in degrees. - * - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose3d object representing the pose, or empty Pose3d if invalid data - */ - public static Pose3d toPose3D(double[] inData) { - if (inData.length < 6) { - // System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d( - Units.degreesToRadians(inData[3]), - Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw - * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles - * are in degrees. - * - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose2d object representing the pose, or empty Pose2d if invalid data - */ - public static Pose2d toPose2D(double[] inData) { - if (inData.length < 6) { - // System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - /** - * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * - * @param pose The Pose3d object to convert - * @return A 6-element array containing [x, y, z, roll, pitch, yaw] - */ - public static double[] pose3dToArray(Pose3d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = pose.getTranslation().getZ(); - result[3] = Units.radiansToDegrees(pose.getRotation().getX()); - result[4] = Units.radiansToDegrees(pose.getRotation().getY()); - result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); - return result; - } - - /** - * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. Note: z, roll, and - * pitch will be 0 since Pose2d only contains x, y, and yaw. - * - * @param pose The Pose2d object to convert - * @return A 6-element array containing [x, y, 0, 0, 0, yaw] - */ - public static double[] pose2dToArray(Pose2d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = 0; - result[3] = Units.radiansToDegrees(0); - result[4] = Units.radiansToDegrees(0); - result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); - return result; - } - - private static double extractArrayEntry(double[] inData, int position) { - if (inData.length < position + 1) { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate( - String limelightName, String entryName, boolean isMegaTag2) { - DoubleArrayEntry poseEntry = - LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); - - TimestampedDoubleArray tsValue = poseEntry.getAtomic(); - double[] poseArray = tsValue.value; - long timestamp = tsValue.timestamp; - - if (poseArray.length == 0) { - // Handle the case where no data is available - return null; // or some default PoseEstimate - } - - var pose = toPose2D(poseArray); - double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int) extractArrayEntry(poseArray, 7); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); - - // Convert server timestamp from microseconds to seconds and adjust for latency - double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for (int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate( - pose, - adjustedTimestamp, - latency, - tagCount, - tagSpan, - tagDist, - tagArea, - rawFiducials, - isMegaTag2); - } - - /** - * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details - */ - public static RawFiducial[] getRawFiducials(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); - var rawFiducialArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 7; - if (rawFiducialArray.length % valsPerEntry != 0) { - return new RawFiducial[0]; - } - - int numFiducials = rawFiducialArray.length / valsPerEntry; - RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; - - for (int i = 0; i < numFiducials; i++) { - int baseIndex = i * valsPerEntry; - int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); - double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); - double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); - double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); - double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); - double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); - double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - - return rawFiducials; - } - - /** - * Gets the latest raw neural detector results from NetworkTables - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details - */ - public static RawDetection[] getRawDetections(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); - var rawDetectionArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 12; - if (rawDetectionArray.length % valsPerEntry != 0) { - return new RawDetection[0]; - } - - int numDetections = rawDetectionArray.length / valsPerEntry; - RawDetection[] rawDetections = new RawDetection[numDetections]; - - for (int i = 0; i < numDetections; i++) { - int baseIndex = i * valsPerEntry; // Starting index for this detection's data - int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); - double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); - double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); - double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); - double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); - double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); - double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); - double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); - double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); - double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); - double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); - double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections[i] = - new RawDetection( - classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, - corner2_Y, corner3_X, corner3_Y); - } - - return rawDetections; - } - - /** - * Prints detailed information about a PoseEstimate to standard output. Includes timestamp, - * latency, tag count, tag span, average tag distance, average tag area, and detailed information - * about each detected fiducial. - * - * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." - */ - public static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static Boolean validPoseEstimate(PoseEstimate pose) { - return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static void Flush() { - NetworkTableInstance.getDefault().flush(); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { - String key = tableName + "/" + entryName; - return doubleArrayEntries.computeIfAbsent( - key, - k -> { - NetworkTable table = getLimelightNTTable(tableName); - return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); - }); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); - } - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - - ///// - ///// - - /** - * Does the Limelight have a valid target? - * - * @param limelightName Name of the Limelight camera ("" for default) - * @return True if a valid target is present, false otherwise - */ - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - /** - * Gets the horizontal offset from the crosshair to the target in degrees. - * - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - /** - * Gets the vertical offset from the crosshair to the target in degrees. - * - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the - * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable - * crosshair functionality. - * - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTXNC(String limelightName) { - return getLimelightNTDouble(limelightName, "txnc"); - } - - /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the - * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable - * crosshair functionality. - * - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTYNC(String limelightName) { - return getLimelightNTDouble(limelightName, "tync"); - } - - /** - * Gets the target area as a percentage of the image (0-100%). - * - * @param limelightName Name of the Limelight camera ("" for default) - * @return Target area percentage (0-100) - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - /** - * T2D is an array that contains several targeting metrcis - * - * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, - * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, - * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, - * targetVerticalExtentPixels, targetSkewDegrees] - */ - public static double[] getT2DArray(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "t2d"); - } - - /** - * Gets the number of targets currently detected. - * - * @param limelightName Name of the Limelight camera - * @return Number of detected targets - */ - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if (t2d.length == 17) { - return (int) t2d[1]; - } - return 0; - } - - /** - * Gets the classifier class index from the currently running neural classifier pipeline - * - * @param limelightName Name of the Limelight camera - * @return Class index from classifier pipeline - */ - public static int getClassifierClassIndex(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if (t2d.length == 17) { - return (int) t2d[10]; - } - return 0; - } - - /** - * Gets the detector class index from the primary result of the currently running neural detector - * pipeline. - * - * @param limelightName Name of the Limelight camera - * @return Class index from detector pipeline - */ - public static int getDetectorClassIndex(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if (t2d.length == 17) { - return (int) t2d[11]; - } - return 0; - } - - /** - * Gets the current neural classifier result class name. - * - * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline - */ - public static String getClassifierClass(String limelightName) { - return getLimelightNTString(limelightName, "tcclass"); - } - - /** - * Gets the primary neural detector result class name. - * - * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline - */ - public static String getDetectorClass(String limelightName) { - return getLimelightNTString(limelightName, "tdclass"); - } - - /** - * Gets the pipeline's processing latency contribution. - * - * @param limelightName Name of the Limelight camera - * @return Pipeline latency in milliseconds - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - /** - * Gets the capture latency. - * - * @param limelightName Name of the Limelight camera - * @return Capture latency in milliseconds - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - /** - * Gets the active pipeline index. - * - * @param limelightName Name of the Limelight camera - * @return Current pipeline index (0-9) - */ - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - /** - * Gets the current pipeline type. - * - * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) - */ - public static String getCurrentPipelineType(String limelightName) { - return getLimelightNTString(limelightName, "getpipetype"); - } - - /** - * Gets the full JSON results dump. - * - * @param limelightName Name of the Limelight camera - * @return JSON string containing all current results - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field - * space - */ - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field - * space - */ - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target - */ - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target - */ - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the camera's coordinate system. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera - */ - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the robot's coordinate system. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot - */ - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot - */ - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); - } - - /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling - * setRobotOrientation() before calling this method. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the RED alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired", false); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the RED alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - } - - /** - * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch, - * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or - * unavailable. - * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data - */ - public static IMUData getIMUData(String limelightName) { - double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); - if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros - } - return new IMUData(imuData); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to be controlled by the current pipeline. - * - * @param limelightName Name of the Limelight camera - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - /** - * Enables standard side-by-side stream mode. - * - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Enables Picture-in-Picture mode with secondary stream in the corner. - * - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Enables Picture-in-Picture mode with primary stream in the corner. - * - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. - * - * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) - */ - public static void setCropWindow( - String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - /** Sets 3D offset point for easy 3D targeting. */ - public static void setFiducial3DOffset( - String limelightName, double offsetX, double offsetY, double offsetZ) { - double[] entries = new double[3]; - entries[0] = offsetX; - entries[1] = offsetY; - entries[2] = offsetZ; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Sets robot orientation values used by MegaTag2 localization algorithm. - * - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second - */ - public static void SetRobotOrientation( - String limelightName, - double yaw, - double yawRate, - double pitch, - double pitchRate, - double roll, - double rollRate) { - SetRobotOrientation_INTERNAL( - limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); - } - - public static void SetRobotOrientation_NoFlush( - String limelightName, - double yaw, - double yawRate, - double pitch, - double pitchRate, - double roll, - double rollRate) { - SetRobotOrientation_INTERNAL( - limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); - } - - private static void SetRobotOrientation_INTERNAL( - String limelightName, - double yaw, - double yawRate, - double pitch, - double pitchRate, - double roll, - double rollRate, - boolean flush) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if (flush) { - Flush(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * Sets the 3D point-of-interest offset for the current fiducial pipeline. - * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking - * - * @param limelightName Name/identifier of the Limelight - * @param x X offset in meters - * @param y Y offset in meters - * @param z Z offset in meters - */ - public static void SetFidcuial3DOffset(String limelightName, double x, double y, double z) { - - double[] entries = new double[3]; - entries[0] = x; - entries[1] = y; - entries[2] = z; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will - * be ignored for robot pose estimation. - * - * @param limelightName Name/identifier of the Limelight - * @param validIDs Array of valid AprilTag IDs to track - */ - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - /** - * Sets the downscaling factor for AprilTag detection. Increasing downscale can improve - * performance at the cost of potentially reduced detection range. - * - * @param limelightName Name/identifier of the Limelight - * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to - * 0 for pipeline control. - */ - public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { - int d = 0; // pipeline - if (downscale == 1.0) { - d = 1; - } - if (downscale == 1.5) { - d = 2; - } - if (downscale == 2) { - d = 3; - } - if (downscale == 3) { - d = 4; - } - if (downscale == 4) { - d = 5; - } - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); - } - - /** - * Sets the camera pose relative to the robot. - * - * @param limelightName Name of the Limelight camera - * @param forward Forward offset in meters - * @param side Side offset in meters - * @param up Up offset in meters - * @param roll Roll angle in degrees - * @param pitch Pitch angle in degrees - * @param yaw Yaw angle in degrees - */ - public static void setCameraPose_RobotSpace( - String limelightName, - double forward, - double side, - double up, - double roll, - double pitch, - double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** Asynchronously take snapshot. */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync( - () -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Gets the latest JSON results output and returns a LimelightResults object. - * - * @param limelightName Name of the Limelight camera - * @return LimelightResults object containing all current target data - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightResults results = new LimelightResults(); - if (mapper == null) { - mapper = - new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index adae354..b6f85b9 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -1,21 +1,29 @@ 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)) { + return getRPM(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 +35,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 +45,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 +151,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/loggers/ProfiledPIDControllerLogger.java b/src/main/java/frc/robot/loggers/ProfiledPIDControllerLogger.java new file mode 100644 index 0000000..25893b8 --- /dev/null +++ b/src/main/java/frc/robot/loggers/ProfiledPIDControllerLogger.java @@ -0,0 +1,25 @@ +package frc.robot.loggers; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.math.controller.ProfiledPIDController; + +@CustomLoggerFor(ProfiledPIDController.class) +public class ProfiledPIDControllerLogger extends ClassSpecificLogger { + public ProfiledPIDControllerLogger() { + super(ProfiledPIDController.class); + } + + @Override + protected void update(EpilogueBackend backend, ProfiledPIDController controller) { + backend.log("P", controller.getP()); + backend.log("I", controller.getI()); + backend.log("D", controller.getD()); + backend.log("At Goal Position", controller.atSetpoint()); + backend.log("Positional Error", controller.getPositionError()); + backend.log("Velocity Error", controller.getVelocityError()); + backend.log("Velocity Tolerance", controller.getVelocityTolerance()); + backend.log("Positional Tolerance", controller.getVelocityError()); + } +} 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); }