diff --git a/src/main/deploy/pathplanner/autos/One Coral Left.auto b/src/main/deploy/pathplanner/autos/One Coral Left.auto new file mode 100644 index 0000000..14000f4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/One Coral Left.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to 60 Right" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Coral L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left.auto b/src/main/deploy/pathplanner/autos/Two Coral Left.auto new file mode 100644 index 0000000..5ca978d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Two Coral Left.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to 60 Right" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Coral L4" + } + }, + { + "type": "path", + "data": { + "pathName": "60 Right to HP" + } + }, + { + "type": "named", + "data": { + "name": "Collect Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "HP to 300 Left" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Coral L4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/300 Right to HP.path b/src/main/deploy/pathplanner/paths/300 Right to HP.path index f84b1d1..98576e0 100644 --- a/src/main/deploy/pathplanner/paths/300 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/300 Right to HP.path @@ -37,12 +37,6 @@ "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ - { - "name": "Score L4", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": null - }, { "name": "HP Pickup", "waypointRelativePos": 0.16666666666666663, diff --git a/src/main/deploy/pathplanner/paths/60 Right to HP.path b/src/main/deploy/pathplanner/paths/60 Right to HP.path new file mode 100644 index 0000000..f6a2c3e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/60 Right to HP.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.988527397260274, + "y": 5.257106164383561 + }, + "prevControl": null, + "nextControl": { + "x": 5.649657534252619, + "y": 6.474186643842743 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3072345890410957, + "y": 7.135316780821918 + }, + "prevControl": { + "x": 2.1636986301369863, + "y": 6.188698630136986 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP Left Position" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "HP Pickup", + "waypointRelativePos": 0.20476190476190542, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.97262661489646 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -120.06858282186238 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to 300 Left.path b/src/main/deploy/pathplanner/paths/HP to 300 Left.path index c12f794..ff41ed9 100644 --- a/src/main/deploy/pathplanner/paths/HP to 300 Left.path +++ b/src/main/deploy/pathplanner/paths/HP to 300 Left.path @@ -12,16 +12,16 @@ "y": 6.909931506849315 }, "isLocked": false, - "linkedName": null + "linkedName": "HP Left Position" }, { "anchor": { - "x": 4.026883561643835, - "y": 5.257106164383561 + "x": 3.973766447368421, + "y": 5.246957236842105 }, "prevControl": { - "x": 3.7113441780821916, - "y": 5.783005136986301 + "x": 3.6582270638067773, + "y": 5.772856209444845 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "Score L4", - "waypointRelativePos": 0, + "name": "Lift L4", + "waypointRelativePos": 0.5, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/deploy/pathplanner/paths/Start to 60 Right.path b/src/main/deploy/pathplanner/paths/Start to 60 Right.path new file mode 100644 index 0000000..f3d5d3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to 60 Right.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.587970890410959, + "y": 6.143621575342466 + }, + "prevControl": null, + "nextControl": { + "x": 6.385916095890411, + "y": 6.158647260273973 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.988527397260274, + "y": 5.227054794520548 + }, + "prevControl": { + "x": 5.574529109589041, + "y": 6.098544520547945 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0.4547619047619047, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -121.60750224624898 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java new file mode 100644 index 0000000..8ed1c02 --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,31 @@ +package frc.robot.constants; + +public class VisionConstants { + // global coordinate map of all tags. index is the tag id. + // Units: inches and degrees. {x, y, z, z-rotation, y-rotation} + // This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS. + public static final double[][] globalTagCoords = {{}, + {656.98, 24.73, 58.50, 126.0, 0}, + {656.98, 291.90, 58.50, 234.0, 0}, + {452.4, 316.21, 51.25, 270, 0}, + {365.2, 241.44, 73.54, 0, 30}, + {365.2, 75.19, 73.54, 0, 30}, + {530.49, 129.97, 12.13, 300, 0}, + {546.87, 158.3, 12.13, 0, 0}, + {530.49, 186.63, 12.13, 60, 0}, + {497.77, 186.63, 12.13, 120, 0}, + {481.39, 158.3, 12.13, 180, 0}, + {497.77, 129.97, 12.13, 240, 0}, + {33.9, 24.73, 58.5, 54, 0}, + {33.9, 291.9, 58.5, 306, 0}, + {325.68, 241.44, 73.54, 180, 30}, + {325.68, 75.19, 73.54, 180, 30}, + {238.49, 0.42, 51.25, 90, 0}, + {160.39, 129.97, 12.13, 240, 0}, + {144.00, 158.3, 12.13, 180, 0}, + {160.39, 186.63, 12.13, 120, 0}, + {193.1, 186.63, 12.13, 60, 0}, + {209.49, 158.3, 12.13, 0, 0}, + {193.1, 129.97, 12.13, 300, 0}, + }; +} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 8d8b1b3..35e5efe 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -1,53 +1,103 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.geometry.Pose2d; +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.networktables.BooleanSubscriber; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.constants.VisionConstants; public class Vision{ - private DoubleSubscriber blackRobotRelativePose; + private DoubleSubscriber blackRobotRelativeX; + private DoubleSubscriber blackRobotRelativeY; + private DoubleSubscriber blackRobotRelativeZ; + private DoubleSubscriber blackClosestTag; private BooleanSubscriber blackTagDetected; private DoubleSubscriber blackFramerate; - private DoubleSubscriber orangeRobotRelativePose; + private DoubleSubscriber orangeRobotRelativeX; + private DoubleSubscriber orangeRobotRelativeY; + private DoubleSubscriber orangeRobotRelativeZ; + private DoubleSubscriber orangeClosestTag; private BooleanSubscriber orangeTagDetected; private DoubleSubscriber orangeFramerate; - public Vision(){ + private DoubleSupplier gyroAngle; + + public Vision(DoubleSupplier gyroAngle){ NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTable blackVisionTable = inst.getTable("black_Fiducial"); NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial"); - blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0); + blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0); + blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0); + blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0); + blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0); blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false); blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0); - orangeRobotRelativePose = orangeVisionTable.getDoubleTopic("orangeRelativePose").subscribe(0.0); + orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0); + orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0); + orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0); + orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0); orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false); orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0); } + + public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){ + Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0], + VisionConstants.globalTagCoords[tagID][1], + new Rotation2d()); - public double getBlackGlobalPose(){ - return blackRobotRelativePose.get(); + Pose2d relative = new Pose2d(relativeCoords, gyroAngle); + + Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation()); + + Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse()); + + return new Pose2d(globalPose.getTranslation(), gyroAngle); } - public double getBlackClosestTag(){ - return blackClosestTag.get(); + public Pose2d getBlackGlobalPose(){ + + return relativeToGlobalPose2d(getBlackClosestTag(), + new Translation2d(getBlackRelativeX(), getBlackRelativeY()), + new Rotation2d(gyroAngle.getAsDouble())); + } + + public double getBlackRelativeX(){ + return blackRobotRelativeX.get(); + } + + public double getBlackRelativeY(){ + return blackRobotRelativeY.get(); + } + + public double getBlackRelativeZ(){ + return blackRobotRelativeZ.get(); + } + + public int getBlackClosestTag(){ + return (int) blackClosestTag.get(); } public double getBlackTimeStamp(){ - return blackRobotRelativePose.getLastChange(); + return blackRobotRelativeX.getLastChange(); } public boolean getBlackTagDetected(){ @@ -57,17 +107,32 @@ public class Vision{ public double getBlackFPS(){ return blackFramerate.get(); } + + public Pose2d getOrangeGlobalPose(){ + + return relativeToGlobalPose2d(getBlackClosestTag(), + new Translation2d(getBlackRelativeX(), getBlackRelativeY()), + new Rotation2d(gyroAngle.getAsDouble())); + } - public double getOrangeGlobalPose(){ - return orangeRobotRelativePose.get(); + public double getOrangeRelativeX(){ + return orangeRobotRelativeX.get(); } - public double getOrangeClosestTag(){ - return orangeClosestTag.get(); + public double getOrangeRelativeY(){ + return orangeRobotRelativeY.get(); + } + + public double getOrangeRelativeZ(){ + return orangeRobotRelativeZ.get(); + } + + public int getOrangeClosestTag(){ + return (int) orangeClosestTag.get(); } public double getOrangeTimeStamp(){ - return orangeRobotRelativePose.getLastChange(); + return orangeRobotRelativeX.getLastChange(); } public boolean getOrangeTagDetected(){