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

two coral auto path good, beak break bad

Browse Source
This commit is contained in:
Team 2648 2025-03-08 20:01:27 -05:00
parent 649660ade6
commit a089dddae3
23 changed files with 263 additions and 98 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"
}
}
]
}

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

@ -13,15 +13,40 @@
{
"type": "named",
"data": {
"name": "Shoot Coral L4"
"name": "Lift L4"
}
},
{
"type": "path",
"data": {
"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"
}
}
]
}
},
{
"type": "named",
"data": {
@ -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,

16
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;
@ -154,12 +155,11 @@ public class RobotContainer {
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,4 +59,6 @@ public class VisionConstants {
{4.993, 2.816, 5.272, 2.996}
};
public static final double latencyFudge = 0.080;
}

36
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;
@ -112,7 +113,10 @@ public class Drivetrain extends SubsystemBase {
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,
@ -164,6 +168,11 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
// if(vision.getOrangeDist() < 72){
// m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
//}
/*
if(vision.getOrangeTagDetected()){
// 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()){
@ -194,6 +203,7 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("black pose", new Pose3d(blackPose));
m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
}
*/
Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
@ -300,23 +310,27 @@ 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()))));
});
}
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));
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;
@ -326,12 +340,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;

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

@ -161,7 +161,7 @@ public class Vision{
}
public double getOrangeTimeStamp(){
return orange_tx.getLastChange();
return orange_tx.getLastChange()-VisionConstants.latencyFudge;
}
public boolean getOrangeTagDetected(){