mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-08 16:28:04 -06:00
end of denver
This commit is contained in:
C4llSiqn
@@ -373,8 +373,8 @@ public final class Constants {
|
||||
// The standard deviations of our vision estimated poses, which affect correction rate
|
||||
// X, Y, Theta
|
||||
// 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> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
|
||||
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
|
||||
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
|
||||
}
|
||||
|
||||
public static final class FieldConstants {
|
||||
|
||||
@@ -130,6 +130,7 @@ public class RobotContainer {
|
||||
|
||||
private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove);
|
||||
// private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed);
|
||||
// private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed);
|
||||
private Command waitDebuger = new waitSupplier(() -> true);
|
||||
|
||||
// private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
|
||||
@@ -138,7 +139,7 @@ public class RobotContainer {
|
||||
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
// true, false);
|
||||
// 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 ConditionalCommand(Commands.none(), new SequentialCommandGroup(
|
||||
@@ -175,11 +176,57 @@ public class RobotContainer {
|
||||
// new ConditionalCommand(
|
||||
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
// () -> m_robotElevator.hasCoral()),
|
||||
// () -> m_robotElevator.hasCoral())
|
||||
|
||||
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(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
@@ -253,7 +300,7 @@ public class RobotContainer {
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
|
||||
); */
|
||||
|
||||
private Command AprilLidarAlignL4Left = new SequentialCommandGroup(
|
||||
private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
// new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
|
||||
@@ -290,8 +337,8 @@ public class RobotContainer {
|
||||
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 ConditionalCommand(
|
||||
// // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
|
||||
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
|
||||
// () -> m_robotElevator.hasCoral()),
|
||||
|
||||
@@ -300,6 +347,51 @@ public class RobotContainer {
|
||||
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(
|
||||
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
|
||||
|
||||
@@ -312,7 +404,7 @@ public class RobotContainer {
|
||||
new waitEndefectorRefrence(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(),
|
||||
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
|
||||
waitDebuger.asProxy(),
|
||||
@@ -504,8 +596,8 @@ public class RobotContainer {
|
||||
), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)),
|
||||
new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)));
|
||||
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right);
|
||||
NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto);
|
||||
NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto);
|
||||
NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left);
|
||||
NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right);
|
||||
NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left);
|
||||
@@ -641,14 +733,14 @@ public class RobotContainer {
|
||||
1,
|
||||
Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
|
||||
),
|
||||
getDeadbandedDriverController().getRight(), 0.30
|
||||
getDeadbandedDriverController().getRight(), 0.15
|
||||
), 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,
|
||||
// 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)
|
||||
.onTrue(thrustIntake.asProxy());
|
||||
@@ -667,10 +759,10 @@ public class RobotContainer {
|
||||
// Button box
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Five)
|
||||
.onTrue(AprilLidarAlignL4Left);
|
||||
.onTrue(AprilLidarAlignL4LeftSemiAuto);
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.One)
|
||||
.onTrue(AprilLidarAlignL4Right);
|
||||
.onTrue(AprilLidarAlignL4RightSemiAuto);
|
||||
|
||||
new JoystickButton(getButtonBox(), ButtonBox.Six)
|
||||
.onTrue(AprilLidarAlignL3Left);
|
||||
@@ -729,10 +821,10 @@ public class RobotContainer {
|
||||
|
||||
// Auto Scoring
|
||||
new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
.onTrue(AprilLidarAlignL4Left);
|
||||
.onTrue(AprilLidarAlignL4LeftSemiAuto);
|
||||
|
||||
new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||
.onTrue(AprilLidarAlignL4Right);
|
||||
.onTrue(AprilLidarAlignL4RightSemiAuto);
|
||||
|
||||
new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode)
|
||||
.onTrue(AprilLidarAlignL3Left);
|
||||
|
||||
@@ -351,7 +351,8 @@ public class SwerveDrive extends Subsystem {
|
||||
rotTarget += deltaAngle;
|
||||
}
|
||||
|
||||
swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
|
||||
vision.addVisionMeasurement(swerveDriveTrain);
|
||||
// swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
|
||||
//back to the ~~future~~ *past*
|
||||
}
|
||||
|
||||
|
||||
@@ -10,6 +10,7 @@ import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
|
||||
import java.time.Instant;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
@@ -49,6 +50,7 @@ public class Vision extends Subsystem {
|
||||
|
||||
private PhotonCamera[] cameras;
|
||||
private PhotonPoseEstimator[] estimators;
|
||||
private List<EstimatedRobotPose> poses = new ArrayList<>();
|
||||
|
||||
private boolean isTagDetected = false;
|
||||
private boolean isTagProcessed = false;
|
||||
@@ -136,7 +138,8 @@ public class Vision extends Subsystem {
|
||||
// double Yaw = 0;
|
||||
double latency = 0;
|
||||
|
||||
Pose2d pose = null;
|
||||
// Pose2d pose = null;
|
||||
poses.clear();
|
||||
|
||||
for(int i = 0; i < cameras.length; i++){
|
||||
PhotonCamera camera = cameras[i];
|
||||
@@ -164,10 +167,12 @@ public class Vision extends Subsystem {
|
||||
if(estimatedRobotPose.isEmpty())
|
||||
continue;
|
||||
|
||||
if(pose == null)
|
||||
pose = estimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
else
|
||||
pose = pose.interpolate(pose, 0.5);
|
||||
poses.add(estimatedRobotPose.get());
|
||||
|
||||
// if(pose == null)
|
||||
// pose = estimatedRobotPose.get().estimatedPose.toPose2d();
|
||||
// else
|
||||
// pose = pose.interpolate(pose, 0.5);
|
||||
// X += pose.getX();
|
||||
// Y += pose.getY();
|
||||
|
||||
@@ -181,43 +186,20 @@ public class Vision extends Subsystem {
|
||||
|
||||
}
|
||||
|
||||
lastLatency = latency / cams;
|
||||
// lastLatency = latency / cams;
|
||||
|
||||
if(isTagProcessed){
|
||||
// Instant now = Instant.now();
|
||||
|
||||
// double curAngle = (Yaw/cams);
|
||||
|
||||
// Pose2d e = new Pose2d();
|
||||
// if(isTagProcessed){
|
||||
|
||||
|
||||
// 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);
|
||||
|
||||
|
||||
// // 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;
|
||||
}
|
||||
// lastVisionTime = now;
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
@@ -247,66 +229,66 @@ public class Vision extends Subsystem {
|
||||
return visionEst; // Will be empty
|
||||
|
||||
visionEst = estimator.update(change);
|
||||
updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
// updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
|
||||
|
||||
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,
|
||||
PhotonPoseEstimator estimator) {
|
||||
if (estimatedPose.isEmpty()) {
|
||||
// No pose input. Default to single-tag std devs
|
||||
curStdDevs = VisionConstants.kSingleTagStdDevs;
|
||||
// /**
|
||||
// * 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,
|
||||
// PhotonPoseEstimator estimator) {
|
||||
// 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;
|
||||
// } 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 = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
if (tagPose.isEmpty()) continue;
|
||||
// // Precalculation - see how many tags we found, and calculate an average-distance metric
|
||||
// for (var tgt : targets) {
|
||||
// var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
|
||||
// if (tagPose.isEmpty()) continue;
|
||||
|
||||
double distance = tagPose
|
||||
.get()
|
||||
.toPose2d()
|
||||
.getTranslation()
|
||||
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
// double distance = tagPose
|
||||
// .get()
|
||||
// .toPose2d()
|
||||
// .getTranslation()
|
||||
// .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
|
||||
|
||||
if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
numTags++;
|
||||
avgDist += distance;
|
||||
}
|
||||
}
|
||||
// if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
|
||||
// numTags++;
|
||||
// avgDist += distance;
|
||||
// }
|
||||
// }
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
// 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
|
||||
@@ -331,11 +313,13 @@ public class Vision extends Subsystem {
|
||||
// }
|
||||
|
||||
public Pose2d getPose2d() {
|
||||
if(isTagDetected && isTagProcessed)
|
||||
// return lastVisionPose;
|
||||
return lastPhysOdomPose;
|
||||
else
|
||||
if(lastPhysOdomPose != null)
|
||||
return lastPhysOdomPose;
|
||||
return new Pose2d();
|
||||
// if(isTagDetected && isTagProcessed)
|
||||
// // return lastVisionPose;
|
||||
// else
|
||||
// return lastPhysOdomPose;
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user