mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
merge swerve branch
This commit is contained in:
Connorppeach
+1
-1
@@ -91,7 +91,7 @@
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
"guid": "78696e70757401000000000000000000"
|
||||
"guid": "Keyboard0"
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -10,6 +10,10 @@ package frc4388.robot;
|
||||
import static edu.wpi.first.units.Units.Inches;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
@@ -27,6 +31,18 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
|
||||
import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
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.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.Distance;
|
||||
@@ -270,6 +286,24 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
public static final String CAMERA_NAME = "Camera_Module_v1";
|
||||
|
||||
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI));
|
||||
|
||||
// public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
|
||||
|
||||
|
||||
public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
|
||||
Arrays.asList(new AprilTag[] {
|
||||
new AprilTag(1, new Pose3d(
|
||||
new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
|
||||
)),
|
||||
}), 100, 100);
|
||||
|
||||
// The standard deviations of our vision estimated poses, which affect correction rate
|
||||
// (Fake values. Experiment and determine estimation noise on an actual robot.)
|
||||
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
|
||||
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
|
||||
}
|
||||
|
||||
public static final class DriveConstants {
|
||||
|
||||
@@ -166,6 +166,13 @@ public class RobotContainer {
|
||||
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
|
||||
// creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time
|
||||
|
||||
// ? /* Programer Buttons (Controller 3)*/
|
||||
|
||||
// * /* Auto Recording */
|
||||
@@ -224,6 +231,8 @@ public class RobotContainer {
|
||||
DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace());
|
||||
return Commands.none();
|
||||
}
|
||||
// zach told me to do the below comment
|
||||
//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -8,6 +8,9 @@
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import org.photonvision.PhotonCamera;
|
||||
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
|
||||
@@ -18,6 +21,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
|
||||
// import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
// import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
// import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
@@ -29,6 +33,8 @@ public class RobotMap {
|
||||
// private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
// public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME);
|
||||
|
||||
public RobotMap() {
|
||||
configureDriveMotorControllers();
|
||||
}
|
||||
|
||||
@@ -0,0 +1,173 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
|
||||
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
|
||||
public class GotoPositionCommand extends Command {
|
||||
|
||||
|
||||
// private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
|
||||
// private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
|
||||
private Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
|
||||
static Gains xygains = new Gains(3,0,0);
|
||||
static Gains rotgains = new Gains(0.1,0,0.0);
|
||||
static double tolerance = 0;
|
||||
|
||||
private PID xPID = new PID(xygains, 0);
|
||||
private PID yPID = new PID(xygains, 0);
|
||||
private PID rotPID = new PID(rotgains, 0);
|
||||
|
||||
SwerveDrive swerveDrive;
|
||||
Vision vision;
|
||||
|
||||
/**
|
||||
* Command to drive robot to position.
|
||||
* @param SwerveDrive m_robotSwerveDrive
|
||||
*/
|
||||
|
||||
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
|
||||
this.swerveDrive = swerveDrive;
|
||||
this.vision = vision;
|
||||
addRequirements(swerveDrive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
xPID.initialize();
|
||||
yPID.initialize();
|
||||
}
|
||||
|
||||
double xerr;
|
||||
double yerr;
|
||||
double roterr;
|
||||
double xytolerance = 0.01;
|
||||
double rottolerance = 1;
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
xerr = targetpos.getX() - vision.getPose2d().getX();
|
||||
yerr = targetpos.getY() - vision.getPose2d().getY();
|
||||
roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
|
||||
|
||||
SmartDashboard.putNumber("PID X Error", xerr);
|
||||
SmartDashboard.putNumber("PID Y Error", yerr);
|
||||
|
||||
double xoutput = xPID.update(xerr);
|
||||
double youtput = yPID.update(yerr);
|
||||
double rotoutput = rotPID.update(roterr);
|
||||
|
||||
Translation2d leftStick = new Translation2d(
|
||||
Math.max(Math.min(youtput, 1), -1),
|
||||
Math.max(Math.min(xoutput, 1), -1)
|
||||
// 0,0
|
||||
);
|
||||
|
||||
Translation2d rightStick = new Translation2d(
|
||||
Math.max(Math.min(rotoutput, 1), -1),
|
||||
0
|
||||
);
|
||||
|
||||
SmartDashboard.putNumber("PID X Output", xoutput);
|
||||
SmartDashboard.putNumber("PID Y Output", youtput);
|
||||
// SmartDashboard.putNumber("PID Y Output", youtput);
|
||||
|
||||
swerveDrive.driveWithInput(leftStick, rightStick, true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance);
|
||||
// this statement is a boolean in and of itself
|
||||
}
|
||||
|
||||
// @Override
|
||||
// public void end(boolean interrupted) {
|
||||
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// @Override
|
||||
// public double getError() {
|
||||
// return e;
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// @Override
|
||||
// public void runWithOutput(double output) {
|
||||
// // TODO Auto-generated method stub
|
||||
// Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0);
|
||||
// Translation2d rightStick = new Translation2d();
|
||||
// // System.out.println("Output = " + -output);
|
||||
// SmartDashboard.putNumber("PID Output", output);
|
||||
// swerveDrive.driveWithInput(leftStick, rightStick, true);
|
||||
// }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private class PID {
|
||||
protected Gains gains;
|
||||
private double output = 0;
|
||||
private double tolerance = 0;
|
||||
|
||||
/** Creates a new PelvicInflammatoryDisease. */
|
||||
public PID(double kp, double ki, double kd, double kf, double tolerance) {
|
||||
gains = new Gains(kp, ki, kd, kf, 0);
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
public PID(Gains gains, double tolerance) {
|
||||
this.gains = gains;
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
public final void initialize() {
|
||||
output = 0;
|
||||
}
|
||||
|
||||
private double prevError, cumError = 0;
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
public double update(double error) {
|
||||
cumError += error * .02; // 20 ms
|
||||
double delta = error - prevError;
|
||||
|
||||
output = error * gains.kP;
|
||||
output += cumError * gains.kI;
|
||||
output += delta * gains.kD;
|
||||
output += gains.kF;
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
// // Returns true when the command should end.
|
||||
// public final boolean isFinished() {
|
||||
// return Math.abs(getError()) < tolerance;
|
||||
// }
|
||||
}
|
||||
}
|
||||
@@ -26,7 +26,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
@@ -34,6 +34,8 @@ import frc4388.utility.Status.ReportLevel;
|
||||
public class SwerveDrive extends Subsystem {
|
||||
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
private Vision vision;
|
||||
|
||||
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
|
||||
private boolean stopped = false;
|
||||
|
||||
@@ -45,16 +47,15 @@ public class SwerveDrive extends Subsystem {
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
private Field2d field = new Field2d();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
|
||||
super();
|
||||
|
||||
SmartDashboard.putData(field);
|
||||
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
this.vision = vision;
|
||||
}
|
||||
|
||||
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
|
||||
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
|
||||
// // rightStick.getAngle()
|
||||
@@ -154,6 +155,23 @@ public class SwerveDrive extends Subsystem {
|
||||
return false;
|
||||
}
|
||||
|
||||
public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
|
||||
// if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
|
||||
// stopModules(); // stop the swerve
|
||||
|
||||
// if (leftStick.getNorm() < 0.05) //if no imput
|
||||
// return; // don't bother doing swerve drive math and return early.
|
||||
|
||||
leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
|
||||
|
||||
swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
|
||||
.withVelocityX(leftStick.getX()*-speedAdjust)
|
||||
.withVelocityY(leftStick.getY()*speedAdjust)
|
||||
.withTargetDirection(rot)
|
||||
);
|
||||
// double
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return swerveDriveTrain.getRotation3d().getAngle();
|
||||
}
|
||||
@@ -172,10 +190,15 @@ public class SwerveDrive extends Subsystem {
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
SmartDashboard.putNumber("RotTartget", rotTarget);
|
||||
|
||||
Optional<Pose2d> e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds());
|
||||
double time = Vision.getTime();
|
||||
|
||||
if(e.isPresent())
|
||||
field.setRobotPose(e.get());
|
||||
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
|
||||
|
||||
if(vision.isTag()){
|
||||
swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
|
||||
}
|
||||
|
||||
// if(e.isPresent())
|
||||
}
|
||||
|
||||
private void reset_index() {
|
||||
|
||||
@@ -0,0 +1,281 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.simulation.PhotonCameraSim;
|
||||
import org.photonvision.simulation.SimCameraProperties;
|
||||
import org.photonvision.simulation.VisionSystemSim;
|
||||
import org.photonvision.targeting.MultiTargetPNPResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
import com.ctre.phoenix6.Utils;
|
||||
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.utility.Status;
|
||||
import frc4388.utility.Subsystem;
|
||||
|
||||
public class Vision extends Subsystem {
|
||||
|
||||
private PhotonCamera camera;
|
||||
|
||||
private boolean isTag = false;
|
||||
private Pose2d lastVisionPose = new Pose2d();
|
||||
private Pose2d lastPhysOdomPose = new Pose2d();
|
||||
|
||||
private Matrix<N3, N1> curStdDevs;
|
||||
private final PhotonPoseEstimator photonEstimator;
|
||||
|
||||
private Field2d field = new Field2d();
|
||||
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
GenericEntry sbTag = subsystemLayout
|
||||
.add("Tag Detected", false)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbCamConnected = subsystemLayout
|
||||
.add("Camera Connnected", false)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
public Vision(PhotonCamera camera){
|
||||
this.camera = camera;
|
||||
SmartDashboard.putData(field);
|
||||
|
||||
photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS);
|
||||
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
var result = camera.getLatestResult();
|
||||
isTag = result.hasTargets();
|
||||
|
||||
// Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
|
||||
|
||||
// if (multitag.isEmpty()) {
|
||||
// Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best;
|
||||
// }else if()
|
||||
|
||||
|
||||
// sbTag.setBoolean(isTag);
|
||||
// field.setRobotPose(getPose2d());
|
||||
|
||||
// sbCamConnected.setBoolean(camera);
|
||||
|
||||
// System.out.println(isTag);
|
||||
|
||||
if(!isTag){
|
||||
sbTag.setBoolean(isTag);
|
||||
field.setRobotPose(getPose2d());
|
||||
return;
|
||||
}
|
||||
|
||||
var EstimatedRobotPose = getEstimatedGlobalPose();
|
||||
|
||||
// In case the pose estimator fails to estimate the pose, fallback to physical odometry.
|
||||
if(EstimatedRobotPose.isEmpty()){
|
||||
isTag = false;
|
||||
sbTag.setBoolean(isTag);
|
||||
field.setRobotPose(getPose2d());
|
||||
return;
|
||||
}
|
||||
|
||||
lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
// lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation()));
|
||||
// lastVisionPose = new Pose2d(
|
||||
// lastVisionPose.getTranslation(),
|
||||
// lastPhysOdomPose.getRotation()
|
||||
// );
|
||||
|
||||
|
||||
|
||||
field.setRobotPose(getPose2d());
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* The latest estimated robot pose on the field from vision data. This may be empty. This should
|
||||
* only be called once per loop.
|
||||
*
|
||||
* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
|
||||
* {@link getEstimationStdDevs}
|
||||
*
|
||||
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
|
||||
* used for estimation.
|
||||
*/
|
||||
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
|
||||
Optional<EstimatedRobotPose> visionEst = Optional.empty();
|
||||
for (var change : camera.getAllUnreadResults()) {
|
||||
visionEst = photonEstimator.update(change);
|
||||
updateEstimationStdDevs(visionEst, change.getTargets());
|
||||
|
||||
// if (Robot.isSimulation()) {
|
||||
// visionEst.ifPresentOrElse(
|
||||
// est ->
|
||||
// getSimDebugField()
|
||||
// .getObject("VisionEstimation")
|
||||
// .setPose(est.estimatedPose.toPose2d()),
|
||||
// () -> {
|
||||
// getSimDebugField().getObject("VisionEstimation").setPoses();
|
||||
// });
|
||||
// }
|
||||
}
|
||||
return visionEst;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
|
||||
* deviations based on number of tags, estimation strategy, and distance from the tags.
|
||||
*
|
||||
* @param estimatedPose The estimated pose to guess standard deviations for.
|
||||
* @param targets All targets in this camera frame
|
||||
*/
|
||||
private void updateEstimationStdDevs(
|
||||
Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
|
||||
if (estimatedPose.isEmpty()) {
|
||||
// No pose input. Default to single-tag std devs
|
||||
curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
|
||||
} else {
|
||||
// Pose present. Start running Heuristic
|
||||
var estStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
int numTags = 0;
|
||||
double avgDist = 0;
|
||||
|
||||
// Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
for (var tgt : targets) {
|
||||
var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
if (tagPose.isEmpty()) continue;
|
||||
numTags++;
|
||||
avgDist +=
|
||||
tagPose
|
||||
.get()
|
||||
.toPose2d()
|
||||
.getTranslation()
|
||||
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
}
|
||||
|
||||
if (numTags == 0) {
|
||||
// No tags visible. Default to single-tag std devs
|
||||
curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
} else {
|
||||
// One or more tags visible, run the full heuristic.
|
||||
avgDist /= numTags;
|
||||
// Decrease std devs if multiple targets are visible
|
||||
if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
|
||||
// Increase std devs based on (average) distance
|
||||
if (numTags == 1 && avgDist > 4)
|
||||
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
|
||||
else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
|
||||
curStdDevs = estStdDevs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the latest standard deviations of the estimated pose from {@link
|
||||
* #getEstimatedGlobalPose()}, for use with {@link
|
||||
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
|
||||
* only be used when there are targets visible.
|
||||
*/
|
||||
public Matrix<N3, N1> getEstimationStdDevs() {
|
||||
return curStdDevs;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
public void setLastOdomPose(Optional<Pose2d> pose){
|
||||
if(pose.isPresent())
|
||||
lastPhysOdomPose = pose.get();
|
||||
}
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(isTag)
|
||||
return lastVisionPose;
|
||||
else
|
||||
return lastPhysOdomPose;
|
||||
}
|
||||
|
||||
public static double getTime() {
|
||||
return Utils.getCurrentTimeSeconds();
|
||||
}
|
||||
|
||||
public boolean isTag(){
|
||||
return isTag;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
return "Vision";
|
||||
}
|
||||
|
||||
// GenericEntry sbShiftState = subsystemLayout
|
||||
// .add("Shift State", 0)
|
||||
// .withWidget(BuiltInWidgets.kNumberBar)
|
||||
// .getEntry();
|
||||
|
||||
|
||||
@Override
|
||||
public void queryStatus() {
|
||||
sbTag.setBoolean(isTag);
|
||||
sbCamConnected.setBoolean(camera.isConnected());
|
||||
// field.setRobotPose(getPose2d());
|
||||
}
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -35,13 +35,13 @@ public class CanDevice {
|
||||
System.out.println(getName() + " - " + str);
|
||||
}
|
||||
|
||||
public Status queryStatus() {
|
||||
Status s = new Status();
|
||||
// public Status queryStatus() {
|
||||
// Status s = new Status();
|
||||
|
||||
s.addReport(ReportLevel.INFO, "TODO");
|
||||
// s.addReport(ReportLevel.INFO, "TODO");
|
||||
|
||||
return s;
|
||||
}
|
||||
// return s;
|
||||
// }
|
||||
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
{
|
||||
"fileName": "PathplannerLib-2025.1.1.json",
|
||||
"name": "PathplannerLib",
|
||||
"version": "2025.1.1",
|
||||
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
|
||||
],
|
||||
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-java",
|
||||
"version": "2025.1.1"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-cpp",
|
||||
"version": "2025.1.1",
|
||||
"libName": "PathplannerLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"linuxarm64"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
{
|
||||
"fileName": "photonlib.json",
|
||||
"name": "photonlib",
|
||||
"version": "v2025.0.0-beta-8",
|
||||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
|
||||
"frcYear": "2025",
|
||||
"mavenUrls": [
|
||||
"https://maven.photonvision.org/repository/internal",
|
||||
"https://maven.photonvision.org/repository/snapshots"
|
||||
],
|
||||
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photontargeting-cpp",
|
||||
"version": "v2025.0.0-beta-8",
|
||||
"skipInvalidPlatforms": true,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photonlib-cpp",
|
||||
"version": "v2025.0.0-beta-8",
|
||||
"libName": "photonlib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photontargeting-cpp",
|
||||
"version": "v2025.0.0-beta-8",
|
||||
"libName": "photontargeting",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photonlib-java",
|
||||
"version": "v2025.0.0-beta-8"
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "photontargeting-java",
|
||||
"version": "v2025.0.0-beta-8"
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user