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

tweaking vision to 6328

Browse Source
This commit is contained in:
Tylr-J42 2025-03-07 02:03:26 -05:00
parent 2ae2beddfa
commit 7c446fd874
1 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTable;
@ -32,8 +33,8 @@ public class Vision{
private DoubleSubscriber orangeFramerate; private DoubleSubscriber orangeFramerate;
private double[] orangeCamPose = {0.0, -5.0, -10, 14.0-7.673, 1.05-14.0, 7.308+2.75}; private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
private double[] blackCamPose = {0.0, -5.0, 10, 14.0-7.673, 1.05-14.0, 7.308+2.75}; private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
public Vision(){ public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTableInstance inst = NetworkTableInstance.getDefault();
@ -75,13 +76,13 @@ public class Vision{
return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get())); return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
} }
public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){ public Pose2d cameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
System.out.println(gyroBuffer.getSample(timestamp)); System.out.println(gyroBuffer.getSample(timestamp));
double distance2d = totalDist * Math.cos(-camPose[1] - ty); double distance2d = totalDist * Math.cos(-camPose[1] - ty);
Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0] - tx)); Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(-tx)));
Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0], Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
VisionConstants.globalTagCoords[tagID][1], VisionConstants.globalTagCoords[tagID][1],
@ -90,17 +91,18 @@ public class Vision{
Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
.transformBy(new Transform2d(distance2d, 0.0, new Rotation2d())) .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
.getTranslation(); .getTranslation();
Pose2d robotPose = new Pose2d(fieldToCameraTranslation, Pose2d robotPose = new Pose2d(
new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0]))) fieldToCameraTranslation,
.transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(camPose[0])), Pose2d.kZero)); new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[2])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(camPose[3], camPose[4]), new Rotation2d(camPose[2])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get())); robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
return robotPose; return robotPose;
} }
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){ public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(), return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose); getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose);
} }
@ -134,7 +136,7 @@ public class Vision{
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){ public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){ if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(), return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
); );
}else{ }else{