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

adjusting vision and odometry fusion

Browse Source
This commit is contained in:
Tylr-J42 2025-03-10 23:37:34 -04:00
commit dd50663b9e
23 changed files with 279 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
.OutlineViewer/outlineviewer.json
View File

@ -2,6 +2,9 @@
"Clients": {
"open": true
},
"Connections": {
"open": true
},
"NetworkTables Settings": {
"mode": "Client (NT4)"
},
@ -14,6 +17,12 @@
},
"open": true
},
"client@5": {
"Publishers": {
"open": true
},
"open": true
},
"outlineviewer@2": {
"Publishers": {
"open": true

18
src/main/deploy/pathplanner/autos/One Coral Left.auto
View File

@ -10,6 +10,18 @@
"pathName": "Start to 30 Right"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "J Approach"
}
},
{
"type": "named",
"data": {
@ -21,6 +33,12 @@
"data": {
"pathName": "J Backup"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}

41
src/main/deploy/pathplanner/autos/Two Coral Left.auto
View File

@ -13,13 +13,38 @@
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "30 Right to HP"
"pathName": "J Approach"
}
},
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "30 Right to HP"
}
},
{
"type": "named",
"data": {
"name": "HP Pickup"
}
}
]
}
},
{
@ -34,6 +59,18 @@
"pathName": "HP to 330 Left"
}
},
{
"type": "named",
"data": {
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"pathName": "K Approach"
}
},
{
"type": "named",
"data": {

2
src/main/deploy/pathplanner/paths/150 Right to HP.path
View File

@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

19
src/main/deploy/pathplanner/paths/30 Right to HP.path
View File

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.3072345890410957,
"y": 7.135316780821918
"x": 1.174795081967213,
"y": 7.321618852459016
},
"prevControl": {
"x": 2.1636986301369863,
"y": 6.188698630136986
"x": 2.031259123063103,
"y": 6.375000701774084
},
"nextControl": null,
"isLocked": false,
@ -31,17 +31,10 @@
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "HP Pickup",
"waypointRelativePos": 0.20476190476190542,
"endWaypointRelativePos": null,
"command": null
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

2
src/main/deploy/pathplanner/paths/330 Right to HP.path
View File

@ -46,7 +46,7 @@
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

2
src/main/deploy/pathplanner/paths/H Backup.path
View File

@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

29
src/main/deploy/pathplanner/paths/HP to 330 Left.path
View File

@ -3,45 +3,38 @@
"waypoints": [
{
"anchor": {
"x": 1.3072345890410957,
"y": 7.135316780821918
"x": 1.174795081967213,
"y": 7.321618852459016
},
"prevControl": null,
"nextControl": {
"x": 2.5994434931506847,
"y": 6.909931506849315
"x": 2.4670039860768025,
"y": 7.096233578486413
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.973766447368421,
"y": 5.246957236842105
"x": 3.6202868852459016,
"y": 5.859118852459017
},
"prevControl": {
"x": 3.6582270638067773,
"y": 5.772856209444845
"x": 3.304747501684258,
"y": 6.385017825061756
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "Before K"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.5,
"endWaypointRelativePos": null,
"command": null
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

54
src/main/deploy/pathplanner/paths/J Approach.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 5.286577868852459,
"y": 5.955020491803278
},
"prevControl": null,
"nextControl": {
"x": 5.166700819672132,
"y": 5.679303278688525
},
"isLocked": false,
"linkedName": "Before J"
},
{
"anchor": {
"x": 4.974897540983607,
"y": 5.115881147540984
},
"prevControl": {
"x": 5.058811475409836,
"y": 5.403586065573771
},
"nextControl": null,
"isLocked": false,
"linkedName": "J"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -119.71497744813712
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -119.71497744813712
},
"useDefaultConstraints": true
}

12
src/main/deploy/pathplanner/paths/J Backup.path
View File

@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 4.988527397260274,
"y": 5.227054794520548
"x": 4.974897540983607,
"y": 5.115881147540984
},
"prevControl": null,
"nextControl": {
"x": 5.1321837955756875,
"y": 5.49468698033176
"x": 5.11855393929902,
"y": 5.383513333352196
},
"isLocked": false,
"linkedName": "J"
@ -34,7 +34,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -48,7 +48,7 @@
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -121.60750224624898
"rotation": -119.71497744813712
},
"useDefaultConstraints": true
}

54
src/main/deploy/pathplanner/paths/K Approach.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.6202868852459016,
"y": 5.859118852459017
},
"prevControl": null,
"nextControl": {
"x": 3.7970635805425386,
"y": 5.68234215716238
},
"isLocked": false,
"linkedName": "Before K"
},
{
"anchor": {
"x": 3.8720286885245896,
"y": 5.043954918032787
},
"prevControl": {
"x": 3.836065573770491,
"y": 5.295696721311475
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -60.94539590092286
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -59.69923999693802
},
"useDefaultConstraints": true
}

2
src/main/deploy/pathplanner/paths/New Path.path
View File

@ -45,7 +45,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

2
src/main/deploy/pathplanner/paths/Right Start to 150 Right.path
View File

@ -41,7 +41,7 @@
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

31
src/main/deploy/pathplanner/paths/Start to 30 Right.path
View File

@ -3,45 +3,38 @@
"waypoints": [
{
"anchor": {
"x": 7.587970890410959,
"y": 6.143621575342466
"x": 7.588217213114754,
"y": 7.537397540983607
},
"prevControl": null,
"nextControl": {
"x": 6.385916095890411,
"y": 6.158647260273973
"x": 6.3534836065573765,
"y": 6.51844262295082
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.988527397260274,
"y": 5.227054794520548
"x": 5.286577868852459,
"y": 5.955020491803278
},
"prevControl": {
"x": 5.574529109589041,
"y": 6.098544520547945
"x": 5.872579581181226,
"y": 6.826510217830675
},
"nextControl": null,
"isLocked": false,
"linkedName": "J"
"linkedName": "Before J"
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Lift L4",
"waypointRelativePos": 0.4547619047619047,
"endWaypointRelativePos": null,
"command": null
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -49,7 +42,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -121.60750224624898
"rotation": -119.71497744813712
},
"reversed": false,
"folder": null,

2
src/main/deploy/pathplanner/paths/Start to H.path
View File

@ -41,7 +41,7 @@
],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

2
src/main/deploy/pathplanner/paths/fein.path
View File

@ -50,7 +50,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 1.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,

2
src/main/deploy/pathplanner/settings.json
View File

@ -5,7 +5,7 @@
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 4.0,
"defaultMaxAccel": 1.0,
"defaultMaxAccel": 2.5,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 400.0,
"defaultNominalVoltage": 12.0,

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

@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.events.EventTrigger;
import com.pathplanner.lib.path.EventMarker;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -153,13 +154,12 @@ public class RobotContainer {
driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
driver.start().whileTrue(drivetrain.resetToVision());
/*
driver.rightBumper().whileTrue(
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3],
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
() -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
)
);
@ -167,10 +167,9 @@ public class RobotContainer {
drivetrain.goToPose(
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1],
() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
() -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3])
)
);
*/
//Operator inputs
operator.povUp().onTrue(
@ -227,16 +226,15 @@ public class RobotContainer {
);
}
private void configureNamedCommands() {
new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
new EventTrigger("Lift L4").whileTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5));
NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
}
//creates tabs and transforms them on the shuffleboard
@ -325,7 +323,7 @@ public class RobotContainer {
apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist());
apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag());
// sensorTab.addDouble(" ID", vision::getOrangeClosestTag);

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

@ -12,7 +12,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
public class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 4;
public static final double kMaxAccelerationMetersPerSecondSquared = 1;
public static final double kMaxAccelerationMetersPerSecondSquared = 2;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;

6
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -57,10 +57,10 @@ public class DrivetrainConstants {
public static final boolean kGyroReversed = true;
public static final double kHeadingP = 0.0;
public static final double kHeadingP = 0.01;
public static final double kXTranslationP = 0.0;
public static final double kYTranslationP = 0.0;
public static final double kXTranslationP = 0.5;
public static final double kYTranslationP = 0.5;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM

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

@ -40,12 +40,12 @@ public class VisionConstants {
{},
{},
{},
{4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
{5.789+12.272, 3.862, 5.789+12.272, 4.194},
{5.275+12.272, 5.075, 4.991+12.272, 5.246},
{3.986+12.272, 5.24, 3.701+12.272, 5.076},
{3.183+12.272, 4.191, 3.183, 3.857},
{3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
{13.570, 2.816, 13.858, 2.970},//6
{14.373, 3.862, 14.385, 4.194},
{13.858, 5.032, 13.558, 5.227},
{12.575, 5.227, 12.287, 5.056},
{11.772, 4.169, 11.772, 3.845},
{12.287, 2.982, 12.587, 2.826},//11
{},
{},
{},
@ -59,6 +59,6 @@ public class VisionConstants {
{4.993, 2.816, 5.272, 2.996}
};
public static final double latencyFudgeFactor = 0;
public static final double latencyFudge = 0;
}

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

@ -22,6 +22,7 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@ -94,6 +95,7 @@ public class Drivetrain extends SubsystemBase {
);
gyro = new AHRS(NavXComType.kMXP_SPI);
gyro.reset();
m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics,
@ -104,16 +106,19 @@ public class Drivetrain extends SubsystemBase {
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
new Pose2d()
new Pose2d(),
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
);
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
pidHeading.enableContinuousInput(-180, 180);
pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
AutoBuilder.configure(
this::getPose,
@ -166,7 +171,13 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
}
// if the detected tags match your alliances reef tags use their pose estimates
if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
@ -180,6 +191,10 @@ public class Drivetrain extends SubsystemBase {
}
if(vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
}
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());
@ -191,6 +206,7 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("black pose", new Pose3d(blackPose2d));
}
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
if(musicTimer.get()>10){
@ -296,10 +312,14 @@ public class Drivetrain extends SubsystemBase {
public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
return run(() -> {
drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()),
drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1),
MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1),
pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()),
true);
Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d(
new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()),
new Rotation2d(headingSetpoint.getAsDouble()))));
});
}
@ -307,12 +327,12 @@ public class Drivetrain extends SubsystemBase {
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));
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][0]), 2)
+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[17][1]), 2));
for(int i = 17; i <= 22; ++i){
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
double distance = Math.sqrt(Math.pow(getPose().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][0]), 2)
+ Math.pow(getPose().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
if(distance < closestTagDist){
closestTag = i;
@ -322,12 +342,12 @@ public class Drivetrain extends SubsystemBase {
return closestTag;
}else{
int closestTag = 6;
double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
double closestTagDist = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][0]), 2)
+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[6][1]), 2));
for(int i = 6; i <= 11; ++i){
double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
double distance = Math.sqrt(Math.pow(m_estimator.getEstimatedPosition().getX()- Units.inchesToMeters( VisionConstants.globalTagCoords[i][0]), 2)
+ Math.pow(m_estimator.getEstimatedPosition().getY()- Units.inchesToMeters(VisionConstants.globalTagCoords[i][1]), 2));
if(distance < closestTagDist){
closestTag = i;

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

@ -15,6 +15,8 @@ import frc.robot.constants.VisionConstants;
public class Vision{
private NetworkTable blackVisionTable;
private DoubleSubscriber black_tx;
private DoubleSubscriber black_ty;
private DoubleSubscriber black_dist;
@ -24,6 +26,8 @@ public class Vision{
private DoubleSubscriber blackFramerate;
private NetworkTable orangeVisionTable;
private DoubleSubscriber orange_tx;
private DoubleSubscriber orange_ty;
private DoubleSubscriber orange_dist;
@ -39,8 +43,8 @@ public class Vision{
public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault();
NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
blackVisionTable = inst.getTable("black_Fiducial");
orangeVisionTable = inst.getTable("orange_Fiducial");
black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
@ -103,7 +107,7 @@ public class Vision{
public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
}
public double getBlackTX(){
@ -123,7 +127,7 @@ public class Vision{
}
public double getBlackTimeStamp(){
return black_tx.getLastChange();
return black_tx.getLastChange()-VisionConstants.latencyFudge;
}
public boolean getBlackTagDetected(){
@ -137,7 +141,7 @@ public class Vision{
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, orangeCamPose
orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
);
}else{
return new Pose2d();
@ -161,7 +165,7 @@ public class Vision{
}
public double getOrangeTimeStamp(){
return orange_tx.getLastChange();
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
}
public boolean getOrangeTagDetected(){