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

vision stuff

Browse Source
This commit is contained in:
Tylr-J42 2025-02-21 04:22:22 -05:00
parent 858c897aad
commit 1c64d7344b
2 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -52,9 +52,10 @@ public class RobotContainer {
climberRollers = new ClimberRollers(); climberRollers = new ClimberRollers();
//vision = new Vision(drivetrain::getGyroValue);
drivetrain = new Drivetrain(); drivetrain = new Drivetrain();
vision = new Vision(drivetrain::getGyroValue);
elevator = new Elevator(); elevator = new Elevator();
//elevator = new ElevatorSysID(); //elevator = new ElevatorSysID();

18
src/main/java/frc/robot/subsystems/Vision.java
View File

@ -40,9 +40,9 @@ public class Vision{
NetworkTable blackVisionTable = inst.getTable("black_Fiducial"); NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial"); NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0); blackRobotRelativeX = blackVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0); blackRobotRelativeY = blackVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0); blackRobotRelativeZ = blackVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0); blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false); blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
@ -59,24 +59,23 @@ public class Vision{
orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0); orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
} }
public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){ public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords){
Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0], Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1], VisionConstants.globalTagCoords[tagID][1],
new Rotation2d()); new Rotation2d());
Pose2d relative = new Pose2d(relativeCoords, gyroAngle); Pose2d relative = new Pose2d(relativeCoords, new Rotation2d(gyroAngle.getAsDouble()));
Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation()); Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse()); Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
return new Pose2d(globalPose.getTranslation(), gyroAngle); return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroAngle.getAsDouble()));
} }
public Pose2d getBlackGlobalPose(){ public Pose2d getBlackGlobalPose(){
return relativeToGlobalPose2d(getBlackClosestTag(), return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()), new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
new Rotation2d(gyroAngle.getAsDouble()));
} }
public double getBlackRelativeX(){ public double getBlackRelativeX(){
@ -109,8 +108,7 @@ public class Vision{
public Pose2d getOrangeGlobalPose(){ public Pose2d getOrangeGlobalPose(){
return relativeToGlobalPose2d(getOrangeClosestTag(), return relativeToGlobalPose2d(getOrangeClosestTag(),
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
new Rotation2d(gyroAngle.getAsDouble()));
} }
public double getOrangeRelativeX(){ public double getOrangeRelativeX(){