Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

end of denver

Browse Source
This commit is contained in:
C4llSiqn
2025-03-31 17:57:30 -06:00
parent ca2e0cb9cd
commit a06b90ca01
4 changed files with 191 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -373,8 +373,8 @@ public final class Constants {
// The standard deviations of our vision estimated poses, which affect correction rate // The standard deviations of our vision estimated poses, which affect correction rate
// X, Y, Theta // X, Y, Theta
// https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(3, 3, 4); public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
} }
public static final class FieldConstants { public static final class FieldConstants {
src/main/java/frc4388/robot/RobotContainer.java
+107 -15
View File
@@ -130,6 +130,7 @@ public class RobotContainer {
private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
// private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
// private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed);
private Command waitDebuger = new waitSupplier(() -> true); private Command waitDebuger = new waitSupplier(() -> true);
// private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
@@ -138,7 +139,7 @@ public class RobotContainer {
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
// true, false); // true, false);
// private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
private Command AprilLidarAlignL4Right = new SequentialCommandGroup( private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
@@ -175,11 +176,57 @@ public class RobotContainer {
// new ConditionalCommand( // new ConditionalCommand(
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
// () -> m_robotElevator.hasCoral()), // () -> m_robotElevator.hasCoral())
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); );
private Command AprilLidarAlignL4RightSemiAuto = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
waitDebuger.asProxy(),
// new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// waitDebuger.asProxy(),
// new ParallelRaceGroup(
// new WaitCommand(1),
// new MoveUntilSuply(
// m_robotSwerveDrive,
// new Translation2d(0,-0.5),
// new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
// ),
// new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
// () -> m_robotElevator.hasCoral())
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command WannaSeeMeDunk = new SequentialCommandGroup(
new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new MoveForTimeCommand(
m_robotSwerveDrive,
new Translation2d(0,1),
new Translation2d(),
AutoConstants.L4_DRIVE_TIME,
true
),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive)
);
/* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
@@ -253,7 +300,7 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); */ ); */
private Command AprilLidarAlignL4Left = new SequentialCommandGroup( private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
@@ -290,8 +337,8 @@ public class RobotContainer {
new MoveForTimeCommand(m_robotSwerveDrive, new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
// new ConditionalCommand( // // new ConditionalCommand(
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
// () -> m_robotElevator.hasCoral()), // () -> m_robotElevator.hasCoral()),
@@ -300,6 +347,51 @@ public class RobotContainer {
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
); );
private Command AprilLidarAlignL4LeftSemiAuto = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
), () -> m_robotElevator.isL4Primed()),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// waitDebuger.asProxy(),
// new ParallelRaceGroup(
// new WaitCommand(1),
// new MoveUntilSuply(
// m_robotSwerveDrive,
// new Translation2d(0,-0.5),
// new Translation2d(), m_robotElevator::getEndeffectorLimit, true)
// ),
// new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
// new waitEndefectorRefrence(m_robotElevator),
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
// // // new ConditionalCommand(
// // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
// // () -> m_robotElevator.hasCoral()),
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
);
private Command AprilLidarAlignL3Left = new SequentialCommandGroup( private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
@@ -312,7 +404,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator), new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT, true), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
waitDebuger.asProxy(), waitDebuger.asProxy(),
new LidarAlign(m_robotSwerveDrive, m_reefLidar), new LidarAlign(m_robotSwerveDrive, m_reefLidar),
waitDebuger.asProxy(), waitDebuger.asProxy(),
@@ -504,8 +596,8 @@ public class RobotContainer {
), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto);
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto);
NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
@@ -641,14 +733,14 @@ public class RobotContainer {
1, 1,
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
), ),
getDeadbandedDriverController().getRight(), 0.30 getDeadbandedDriverController().getRight(), 0.15
), m_robotSwerveDrive)) ), m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive,
// new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
// // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); .onTrue(WannaSeeMeDunk.asProxy());
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(thrustIntake.asProxy()); .onTrue(thrustIntake.asProxy());
@@ -667,10 +759,10 @@ public class RobotContainer {
// Button box // Button box
new JoystickButton(getButtonBox(), ButtonBox.Five) new JoystickButton(getButtonBox(), ButtonBox.Five)
.onTrue(AprilLidarAlignL4Left); .onTrue(AprilLidarAlignL4LeftSemiAuto);
new JoystickButton(getButtonBox(), ButtonBox.One) new JoystickButton(getButtonBox(), ButtonBox.One)
.onTrue(AprilLidarAlignL4Right); .onTrue(AprilLidarAlignL4RightSemiAuto);
new JoystickButton(getButtonBox(), ButtonBox.Six) new JoystickButton(getButtonBox(), ButtonBox.Six)
.onTrue(AprilLidarAlignL3Left); .onTrue(AprilLidarAlignL3Left);
@@ -729,10 +821,10 @@ public class RobotContainer {
// Auto Scoring // Auto Scoring
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
.onTrue(AprilLidarAlignL4Left); .onTrue(AprilLidarAlignL4LeftSemiAuto);
new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
.onTrue(AprilLidarAlignL4Right); .onTrue(AprilLidarAlignL4RightSemiAuto);
new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode) new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode)
.onTrue(AprilLidarAlignL3Left); .onTrue(AprilLidarAlignL3Left);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+2 -1
View File
@@ -351,7 +351,8 @@ public class SwerveDrive extends Subsystem {
rotTarget += deltaAngle; rotTarget += deltaAngle;
} }
swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); vision.addVisionMeasurement(swerveDriveTrain);
// swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
//back to the ~~future~~ *past* //back to the ~~future~~ *past*
} }
src/main/java/frc4388/robot/subsystems/Vision.java
+79 -91
View File
@@ -10,6 +10,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N3;
import java.time.Instant; import java.time.Instant;
import java.util.ArrayList;
import java.util.List; import java.util.List;
import java.util.Optional; import java.util.Optional;
@@ -49,6 +50,7 @@ public class Vision extends Subsystem {
private PhotonCamera[] cameras; private PhotonCamera[] cameras;
private PhotonPoseEstimator[] estimators; private PhotonPoseEstimator[] estimators;
private List<EstimatedRobotPose> poses = new ArrayList<>();
private boolean isTagDetected = false; private boolean isTagDetected = false;
private boolean isTagProcessed = false; private boolean isTagProcessed = false;
@@ -136,7 +138,8 @@ public class Vision extends Subsystem {
// double Yaw = 0; // double Yaw = 0;
double latency = 0; double latency = 0;
Pose2d pose = null; // Pose2d pose = null;
poses.clear();
for(int i = 0; i < cameras.length; i++){ for(int i = 0; i < cameras.length; i++){
PhotonCamera camera = cameras[i]; PhotonCamera camera = cameras[i];
@@ -164,10 +167,12 @@ public class Vision extends Subsystem {
if(estimatedRobotPose.isEmpty()) if(estimatedRobotPose.isEmpty())
continue; continue;
if(pose == null) poses.add(estimatedRobotPose.get());
pose = estimatedRobotPose.get().estimatedPose.toPose2d();
else // if(pose == null)
pose = pose.interpolate(pose, 0.5); // pose = estimatedRobotPose.get().estimatedPose.toPose2d();
// else
// pose = pose.interpolate(pose, 0.5);
// X += pose.getX(); // X += pose.getX();
// Y += pose.getY(); // Y += pose.getY();
@@ -181,43 +186,20 @@ public class Vision extends Subsystem {
} }
lastLatency = latency / cams; // lastLatency = latency / cams;
if(isTagProcessed){ // if(isTagProcessed){
// Instant now = Instant.now();
// double curAngle = (Yaw/cams);
// Pose2d e = new Pose2d();
// lastVisionPose = pose;
// // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
// // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
// // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
// // SmartDashboard.putNumber("Rotations", rotations);
// lastVisionTime = now;
// // double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise
// if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){
// double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360;
// if(diff > 180){
// rotations -= 1;
// }else if(diff < -180){
// rotations += 1;
// } // }
// }
lastVisionPose = pose;
// lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
// lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
// SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
// SmartDashboard.putNumber("Rotations", rotations);
lastVisionTime = now;
}
} }
@@ -247,66 +229,66 @@ public class Vision extends Subsystem {
return visionEst; // Will be empty return visionEst; // Will be empty
visionEst = estimator.update(change); visionEst = estimator.update(change);
updateEstimationStdDevs(visionEst, change.getTargets(), estimator); // updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
return visionEst; return visionEst;
} }
/** // /**
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard // * 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. // * deviations based on number of tags, estimation strategy, and distance from the tags.
* // *
* @param estimatedPose The estimated pose to guess standard deviations for. // * @param estimatedPose The estimated pose to guess standard deviations for.
* @param targets All targets in this camera frame // * @param targets All targets in this camera frame
*/ // */
private void updateEstimationStdDevs( // private void updateEstimationStdDevs(
Optional<EstimatedRobotPose> estimatedPose, // Optional<EstimatedRobotPose> estimatedPose,
List<PhotonTrackedTarget> targets, // List<PhotonTrackedTarget> targets,
PhotonPoseEstimator estimator) { // PhotonPoseEstimator estimator) {
if (estimatedPose.isEmpty()) { // if (estimatedPose.isEmpty()) {
// No pose input. Default to single-tag std devs // // No pose input. Default to single-tag std devs
curStdDevs = VisionConstants.kSingleTagStdDevs; // curStdDevs = VisionConstants.kSingleTagStdDevs;
} else { // } else {
// Pose present. Start running Heuristic // // Pose present. Start running Heuristic
var estStdDevs = VisionConstants.kSingleTagStdDevs; // var estStdDevs = VisionConstants.kSingleTagStdDevs;
int numTags = 0; // int numTags = 0;
double avgDist = 0; // double avgDist = 0;
// Precalculation - see how many tags we found, and calculate an average-distance metric // // Precalculation - see how many tags we found, and calculate an average-distance metric
for (var tgt : targets) { // for (var tgt : targets) {
var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
if (tagPose.isEmpty()) continue; // if (tagPose.isEmpty()) continue;
double distance = tagPose // double distance = tagPose
.get() // .get()
.toPose2d() // .toPose2d()
.getTranslation() // .getTranslation()
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
numTags++; // numTags++;
avgDist += distance; // avgDist += distance;
} // }
} // }
if (numTags == 0) { // if (numTags == 0) {
// No tags visible. Default to single-tag std devs // // No tags visible. Default to single-tag std devs
curStdDevs = VisionConstants.kSingleTagStdDevs; // curStdDevs = VisionConstants.kSingleTagStdDevs;
} else { // } else {
// One or more tags visible, run the full heuristic. // // One or more tags visible, run the full heuristic.
avgDist /= numTags; // avgDist /= numTags;
// Decrease std devs if multiple targets are visible // // Decrease std devs if multiple targets are visible
if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
// Increase std devs based on (average) distance // // Increase std devs based on (average) distance
if (numTags == 1 && avgDist > 4) // if (numTags == 1 && avgDist > 4)
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
curStdDevs = estStdDevs; // curStdDevs = estStdDevs;
} // }
} // }
} // }
/** /**
* Returns the latest standard deviations of the estimated pose from {@link * Returns the latest standard deviations of the estimated pose from {@link
@@ -331,11 +313,13 @@ public class Vision extends Subsystem {
// } // }
public Pose2d getPose2d() { public Pose2d getPose2d() {
if(isTagDetected && isTagProcessed) if(lastPhysOdomPose != null)
// return lastVisionPose;
return lastPhysOdomPose;
else
return lastPhysOdomPose; return lastPhysOdomPose;
return new Pose2d();
// if(isTagDetected && isTagProcessed)
// // return lastVisionPose;
// else
// return lastPhysOdomPose;
} }
public static double getTime() { public static double getTime() {
@@ -347,7 +331,11 @@ public class Vision extends Subsystem {
} }
public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
for(EstimatedRobotPose pose : poses){
drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
}
}