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

it gives values don't know if they are right

Browse Source
This commit is contained in:
Team 2648 2025-03-06 18:57:14 -05:00
parent aecc342dc4
commit 80b5908206
3 changed files with 62 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
.OutlineViewer/outlineviewer.json
View File

@ -1,13 +1,40 @@
{
"Clients": {
"open": true
},
"NetworkTables Settings": {
"mode": "Client (NT4)"
},
"client@2": {
"open": true
},
"client@4": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@2": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@3": {
"open": true
},
"shuffleboard@1": {
"open": true
},
"transitory": {
"Shuffleboard": {
"Sensors Tab": {
"open": true
},
"open": true
},
"orange_Fiducial": {
"open": true
}
}
}

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

@ -321,10 +321,13 @@ public class RobotContainer {
//sensorTab.add("odometry", drivetrain::getPose);
sensorTab.addDouble("Orange ID", vision::getOrangeClosestTag);
sensorTab.addDouble("Orange x", vision::getOrangeRelativeX);
sensorTab.addDouble("Orange y", vision::getOrangeRelativeY);
sensorTab.addDouble("Orange z", vision::getOrangeRelativeZ);
apriltagTab.addDouble("Orange ID", vision::getOrangeClosestTag);
apriltagTab.addDouble("Orange tx", vision::getOrangeTX);
apriltagTab.addDouble("Orange ty", vision::getOrangeTY);
apriltagTab.addDouble("Orange dist", vision::getOrangeDist);
apriltagTab.addDouble("global x", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
apriltagTab.addDouble("global y", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);

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

@ -8,10 +8,12 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
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.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.constants.VisionConstants;
public class Vision{
@ -25,9 +27,9 @@ public class Vision{
private DoubleSubscriber blackFramerate;
private DoubleSubscriber orangeRobotRelativeX;
private DoubleSubscriber orangeRobotRelativeY;
private DoubleSubscriber orangeRobotRelativeZ;
private DoubleSubscriber orange_tx;
private DoubleSubscriber orange_ty;
private DoubleSubscriber orange_dist;
private DoubleSubscriber orangeClosestTag;
private BooleanSubscriber orangeTagDetected;
@ -52,9 +54,9 @@ public class Vision{
blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("tag3tx").subscribe(0.0);
orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("tag_tvec_y").subscribe(0.0);
orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("tag_tvec_z").subscribe(0.0);
orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
@ -78,6 +80,9 @@ public class Vision{
}
public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
System.out.println(gyroBuffer.getSample(timestamp));
double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty);
Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx));
@ -99,8 +104,8 @@ public class Vision{
}
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return relativeToGlobalPose2d(getBlackClosestTag(),
new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer);
}
public double getBlackRelativeX(){
@ -131,21 +136,26 @@ public class Vision{
return blackFramerate.get();
}
public Pose2d getOrangeGlobalPose(){
return relativeToGlobalPose2d(getOrangeClosestTag(),
new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer
);
}else{
return new Pose2d();
}
}
public double getOrangeRelativeX(){
return orangeRobotRelativeX.get();
public double getOrangeTX(){
return orange_tx.get();
}
public double getOrangeRelativeY(){
return orangeRobotRelativeY.get();
public double getOrangeTY(){
return orange_ty.get();
}
public double getOrangeRelativeZ(){
return orangeRobotRelativeZ.get();
public double getOrangeDist(){
return orange_dist.get();
}
public int getOrangeClosestTag(){
@ -153,7 +163,7 @@ public class Vision{
}
public double getOrangeTimeStamp(){
return orangeRobotRelativeX.getLastChange();
return orange_tx.getLastChange();
}
public boolean getOrangeTagDetected(){