Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

More PhotonVision work, a bunch of comments/docstrings

Browse Source
This commit is contained in:
Bradley Bickford
2026-02-14 22:05:29 -05:00
parent 91a5281202
commit 7f291e42a1
9 changed files with 192 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
src/main/java/frc/robot/RobotContainer.java
View File

@@ -6,11 +6,12 @@ package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -52,6 +53,12 @@ public class RobotContainer {
shooter = new Shooter();
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
vision.addPoseEstimateConsumer((vp) -> {
Logger.recordOutput(
"Vision/" + vp.cameraName() + "/Pose",
vp.visualPose()
);
});
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
@@ -94,7 +101,7 @@ public class RobotContainer {
Pose2d hubPose = Utilities.getHubPose();
double distance = drivetrainPose.getTranslation()
.plus(CompetitionConstants.KRobotToShooter.getTranslation().toTranslation2d())
.plus(CompetitionConstants.kRobotToShooter.getTranslation().toTranslation2d())
.getDistance(hubPose.getTranslation());
if(HoodConstants.kUseInterpolatorForAngle) {
@@ -136,6 +143,31 @@ public class RobotContainer {
}
}
/**
* The "shift display" relies on Elastic's ability to show 1 or more colors
* in a box on the dashboard.
*
* Using the RobotModeTriggers and a Timer based on the FPGA, a reasonably
* accurate "shift display" can be created to indicate whose hub is active
* and when.
*
* During autonomous, and the first 10 seconds of teleop, the shift display
* will display a gradient of both red and blue, indicating the fact that
* both hubs are active.
*
* For the rest of teleop, with the exception of the endgame, the display
* will present either the color red, or the color blue, based on the returned
* value of Utilities.whoHasFirstShift(). Because shifts change on a known cycle,
* we can use the known state of who has first shift, to determine the remaining three shifts
* that come after.
*
* For the endgame portion of teleop, the shift display returns to the gradient
* of both red and blue.
*
* Because this relies on the RobotModeTriggers and an FPGA timer, it should be
* <i>reasonably</i> accurate, it's unlikely to be perfect relative to field time
* but it will be very very very (likely unnoticably) close.
*/
private void configureShiftDisplay() {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);

9
src/main/java/frc/robot/constants/CompetitionConstants.java
View File

@@ -18,7 +18,7 @@ public class CompetitionConstants {
public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
// TODO Real Values
public static final Transform3d KRobotToShooter = new Transform3d();
public static final Transform3d kRobotToShooter = new Transform3d();
public static final Pose2d kBlueHubLocation = new Pose2d(
Units.inchesToMeters(182.11),
@@ -26,6 +26,13 @@ public class CompetitionConstants {
Rotation2d.fromDegrees(0)
);
// TODO The origination value produced by the April Tag Field Layout object may
// influence what this value should actually be. See AprilTagFieldLayout.getOrigin
// For now, the X axis position (forward/backward) is calculated as though the blue
// alliance wall right hand side is the originiation point, so, the distance from
// the blue alliance wall, to the blue alliance hub center point, plus
// the distance between the center of the blue alliance hub and the center of
// the red alliance hub
public static final Pose2d kRedHubLocation = new Pose2d(
Units.inchesToMeters(182.11 + 143.5 * 2),
Units.inchesToMeters(158.84),

39
src/main/java/frc/robot/interfaces/IAprilTagProvider.java
View File

@@ -1,39 +0,0 @@
package frc.robot.interfaces;
import java.util.OptionalDouble;
/**
* An interface which ensures a class can provide common AprilTag oriented
* information from various sources in a consistent way.
*/
public interface IAprilTagProvider {
/**
* A method to get the tags currently in the camera's field of view
* @return
*/
public int[] getVisibleTagIDs();
/**
* A method to get the distance from <i>the camera</i> to the AprilTag specified
*
* @param id The ID of the AprilTag to give a distance to
* @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
*/
public OptionalDouble getTagDistanceFromCameraByID(int id);
/**
* A method to get the pitch from the center of the image of a particular AprilTag
*
* @param id The ID of the AprilTag to get the pitch of
* @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
*/
public OptionalDouble getTagPitchByID(int id);
/**
* A method to get the yaw from the center of the image of a particular AprilTag
*
* @param id The ID of the AprilTag to get the yaw of
* @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
*/
public OptionalDouble getTagYawByID(int id);
}

43
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@@ -21,15 +21,14 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.CompetitionConstants;
import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants;
import frc.robot.utilities.SwerveModule;
import frc.robot.utilities.Utilities;
import frc.robot.utilities.VisualPose;
public class Drivetrain extends SubsystemBase {
@@ -180,6 +179,19 @@ public class Drivetrain extends SubsystemBase {
});
}
/**
* Rotates the robot to a face a given Pose2d position on the field
*
* Note that this Command does not provide a means of timeout. If you are
* using this in an auto context, this Command should be decorated with
* withTimeout(<some_value>). Otherwise, you will be waiting for the PID
* Controller doing the work to report that it is at the desired setpoint.
*
* @param targetPose The Pose2d object to rotate the robot towards
* @param rotate180 When false, the front of the robot faces the specified pose, when true
* the back of the robot faces the specified pose
* @return A complete Command structure that performs the specified action
*/
public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
return runOnce(yawRotationController::reset).andThen(
drive(
@@ -205,21 +217,30 @@ public class Drivetrain extends SubsystemBase {
.until(yawRotationController::atSetpoint);
}
/**
* Locks the robots rotation to face the Alliance Hub on the field.
*
* This method is innately aware of which hub to face based on the assigned alliance color.
*
* This method is <i>NOT</i> for autonomous, see rotateToPose
*
* This method provides a field oriented mechanism of driving the robot, such that the robot
* is always facing the point on the field that is the center of the alliance hub. This
* method assumes that the robots estimated pose is reasonably accurate.
*
* @param xSpeed The X (forward/backward) translational speed of the robot
* @param ySpeed The Y (left/right) translational speed of the robot
* @param rotate180 When false, the front of the robot faces the hub, when true, the back
* of the robot faces the hub
* @return A complete Command structure that performs the specified action
*/
public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
return runOnce(yawRotationController::reset).andThen(
drive(
xSpeed,
ySpeed,
() -> {
Optional<Alliance> alliance = DriverStation.getAlliance();
if(alliance.isEmpty()) {
return 0;
}
Pose2d faceTowards = alliance.get() == Alliance.Blue ?
CompetitionConstants.kBlueHubLocation :
CompetitionConstants.kRedHubLocation;
Pose2d faceTowards = Utilities.getHubPose();
Rotation2d targetRotation = new Rotation2d(
faceTowards.getX() - getPose().getX(),

104
src/main/java/frc/robot/subsystems/PhotonVision.java
View File

@@ -1,32 +1,47 @@
package frc.robot.subsystems;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.function.Consumer;
import java.util.stream.Stream;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.PhotonUtils;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.CompetitionConstants;
import frc.robot.constants.PhotonConstants;
import frc.robot.interfaces.IAprilTagProvider;
import frc.robot.utilities.PhotonVisionConfig;
import frc.robot.utilities.VisualPose;
/**
* This "Subsystem" is not actually a Subsystem. The intent is for this to be treated as
* a "resource", that is, something that is not inherently a physical mechanism to be controlled.
*
* A "resource" in this instance should be thought of as something that can be safely shared
* by other Subsystems generally without collision if more that one Subsystem requires the
* "resource" at any given time.
*
* Resources should <i>NOT</i> produce Commands, they should not have a default Command.
* Resources do not have behaviors, and because Commands are in of themselves behaviors,
* this class should not have Commands.
*
* Part of the thinking behind creating the PhotonVision components this way is to rely
* on the CommandScheduler to call periodic. If this weren't the case, some other subsystem
* would have to manage calling for periodic updates, while still sharing the resource with
* other subsystems <i>somehow</i>.
*
* This class is dynamic, by adding or removing PhotonVisionConfig objects to the "configs"
* List in the PhotonConstants file, you change what is set up internally in this class.
* 1 config means 1 camera, 1 estimator, 1 stored pipeline result, 2 configs means 2 cameras,
* 2 estimators, etc. etc.
*/
public class PhotonVision extends SubsystemBase {
private PhotonCamera[] cameras;
private PhotonPoseEstimator[] estimators;
@@ -65,6 +80,7 @@ public class PhotonVision extends SubsystemBase {
if(!pose.isEmpty()) {
VisualPose visualPose = new VisualPose(
cameras[i].getName(),
pose.get().estimatedPose.toPose2d(),
pose.get().timestampSeconds
);
@@ -76,22 +92,68 @@ public class PhotonVision extends SubsystemBase {
}
}
public void testMethod(int targetID) {
Optional<PhotonTrackedTarget> target = latestResults.stream()
.filter((p) -> p != null)
.map(PhotonPipelineResult::getTargets)
.map(List::stream)
.reduce(Stream::concat)
.get()
.filter((p) -> p.getFiducialId() == targetID)
.max(
Comparator.comparingDouble((ptt) -> {
return (double)ptt.getDetectedObjectConfidence();
})
);
/**
* Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot.
*
* All cameras fields of view are observed, if no camera can see the given tag ID, this
* method will return Optional.empty().
*
* Note that this method has no minimum confidence threshold for a tag. This means that
* if one camera thinks it sees the tag, even with very low confidence, it'll still return
* some sort of pose.
*
* @param tagID The ID of the tag to look for in the latest results from all cameras
* @return An Optional object containing a Pose3d object, or Optional.empty() if
* the tag is not present anywhere in the robots field of view.
*/
public Optional<Pose3d> getBestPoseForTag(int tagID) {
PhotonVisionConfig config = null;
Transform3d bestCameraToTarget = null;
float bestConfidence = -1;
for(int cameraIndex = 0; cameraIndex < latestResults.size(); cameraIndex++) {
if(latestResults.get(cameraIndex) == null) {
continue;
}
for(PhotonTrackedTarget target: latestResults.get(cameraIndex).getTargets()) {
if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectConfidence()) {
continue;
}
config = PhotonConstants.configs.get(cameraIndex);
bestCameraToTarget = target.bestCameraToTarget;
bestConfidence = target.getDetectedObjectConfidence();
}
}
if(bestCameraToTarget == null) {
return Optional.empty();
}
// This is based on what PhotonVision does for multitag Pose estimation
// See PhotonPoseEstimator.multiTagOnCoprocStrategy
// TODO This doesn't currently account for the offset of the tag relative to say the hub
return Optional.of(Pose3d.kZero
.plus(bestCameraToTarget.inverse())
.relativeTo(CompetitionConstants.kTagLayout.getOrigin())
.plus(config.robotToCamera().inverse())
.plus(CompetitionConstants.kRobotToShooter));
}
/**
* Add a Consumer of VisualPose records to the PhotonVision resource.
*
* Each consumer will receive a VisualPose object when any camera produces a new
* VisualPose.
*
* The number of Poses produced in a given 20ms cycle is the same number as how many
* cameras there are on the robot, as currently all cameras configuration will generate
* a Pose2d
*
* @param consumer The lambda or functional reference that will consume Poses produced
* by the PhotonVision resource.
*/
public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
poseEstimateConsumers.add(consumer);
}

14
src/main/java/frc/robot/utilities/SparkMAXTester.java
View File

@@ -8,13 +8,27 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
/**
* A simple subsystem that can be used to test a single SparkMax and associated NEO motor
*/
public class SparkMAXTester extends SubsystemBase {
private SparkMax spark;
/**
* Constructor
*
* @param deviceID The CAN ID of the SparkMAX that needs testing
*/
public SparkMAXTester(int deviceID) {
spark = new SparkMax(deviceID, MotorType.kBrushless);
}
/**
* Sets the speed of the motor
*
* @param speed A method or lambda which returns a double between -1 and 1
* @return A Command object that runs indefinitely to control motor speed
*/
public Command setSpeed(DoubleSupplier speed) {
return run(() -> {
spark.set(speed.getAsDouble());

2
src/main/java/frc/robot/utilities/SwerveModule.java
View File

@@ -145,6 +145,8 @@ public class SwerveModule {
Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
// TODO Re-enable this? Was turned off when there was drivetrain issues
// Now that there aren't, do we try this again?
/*
if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {

18
src/main/java/frc/robot/utilities/Utilities.java
View File

@@ -10,6 +10,13 @@ import frc.robot.constants.CompetitionConstants;
public class Utilities {
public static final double kG = -9.81;
/**
* Returns the Alliance enumeration that indicates who will have the first
* shift. Returns null if the data is not available.
*
* @return The Alliance that will have the first shift, or null if game specific data
* is not present
*/
public static Alliance whoHasFirstShift() {
String gameData = DriverStation.getGameSpecificMessage();
@@ -27,6 +34,13 @@ public class Utilities {
return null;
}
/**
* Returns the pose of the hub for the given alliance assigned to our robot.
* If no alliance is assigned (which is unlikely) this method returns
* the Blue hub pose, which is the closet to the field origin
*
* @return The Pose2d object which represents the appropriate pose of the Hub
*/
public static Pose2d getHubPose() {
Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -44,8 +58,6 @@ public class Utilities {
* Note that X in this scenario is the physical distance from the shooter exit to
* target. Y is the change in height from the shooter exit to the target height
*
* TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
*
* @param targetVMPS The target velocity of the shooter in Meters per Second
* @param deltaXM The "as the crow flies" distance between the shooter exit and the target
* @param deltaYM The height difference between the shooter exit and the target
@@ -66,8 +78,6 @@ public class Utilities {
* Setting softerShot to true changes the angle of attack to a soft, long range shot. False
* makes the shot more of a lob
*
* TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
*
* @param targetVMPS The target velocity of the shooter in Meters per Second
* @param deltaXM The "as the crow flies" distance between the shooter exit and the target
* @param deltaYM The height difference between the shooter exit and the target

6
src/main/java/frc/robot/utilities/VisualPose.java
View File

@@ -2,4 +2,8 @@ package frc.robot.utilities;
import edu.wpi.first.math.geometry.Pose2d;
public record VisualPose(Pose2d visualPose, double timestamp) {}
/**
* A record class which represents the source of a visual pose, the pose itself
* and the timestamp the pose was generated.
*/
public record VisualPose(String cameraName, Pose2d visualPose, double timestamp) {}