Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

small constants change for vision

Browse Source
This commit is contained in:
Tylr-J42 2025-03-08 09:06:49 -05:00
parent 649660ade6
commit 66a9608006
2 changed files with 14 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -59,4 +59,6 @@ public class VisionConstants {
{4.993, 2.816, 5.272, 2.996}
};
public static final double latencyFudgeFactor = 0;
}

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

@ -65,6 +65,7 @@ public class Drivetrain extends SubsystemBase {
public Vision vision;
public Pose2d orangePose2d;
public Pose2d blackPose2d;
/** Creates a new DriveSubsystem. */
public Drivetrain() {
@ -148,6 +149,7 @@ public class Drivetrain extends SubsystemBase {
vision = new Vision();
orangePose2d = new Pose2d();
blackPose2d = new Pose2d();
}
@Override
@ -175,24 +177,18 @@ public class Drivetrain extends SubsystemBase {
m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
}
Logger.recordOutput("orange pose", new Pose3d());
if(orangePose2d.equals(null)){
Logger.recordOutput("orange pose", new Pose3d());
}else{
Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
}
Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
if(vision.getBlackTagDetected()){
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
}else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
}
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
}
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
@ -309,7 +305,7 @@ public class Drivetrain extends SubsystemBase {
public int getClosestTag(){
if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
int closestTag = 17;
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));