diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json index c9869ba..e589b1b 100644 --- a/.OutlineViewer/outlineviewer.json +++ b/.OutlineViewer/outlineviewer.json @@ -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 diff --git a/src/main/deploy/pathplanner/autos/One Coral Left.auto b/src/main/deploy/pathplanner/autos/One Coral Left.auto index d1e9060..e3d7f4c 100644 --- a/src/main/deploy/pathplanner/autos/One Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/One Coral Left.auto @@ -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" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left.auto b/src/main/deploy/pathplanner/autos/Two Coral Left.auto index 3e60427..2acbf2e 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/Two Coral Left.auto @@ -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": { diff --git a/src/main/deploy/pathplanner/paths/150 Right to HP.path b/src/main/deploy/pathplanner/paths/150 Right to HP.path index b648396..00d9fa7 100644 --- a/src/main/deploy/pathplanner/paths/150 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/150 Right to HP.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index b13eb80..fe904b3 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -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, diff --git a/src/main/deploy/pathplanner/paths/330 Right to HP.path b/src/main/deploy/pathplanner/paths/330 Right to HP.path index e99b2be..cf9a6de 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/H Backup.path b/src/main/deploy/pathplanner/paths/H Backup.path index ce72262..1b8f1f1 100644 --- a/src/main/deploy/pathplanner/paths/H Backup.path +++ b/src/main/deploy/pathplanner/paths/H Backup.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Left.path b/src/main/deploy/pathplanner/paths/HP to 330 Left.path index d311ea1..7136848 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Left.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Left.path @@ -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, diff --git a/src/main/deploy/pathplanner/paths/J Approach.path b/src/main/deploy/pathplanner/paths/J Approach.path new file mode 100644 index 0000000..0caf4b8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/J Approach.path @@ -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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J Backup.path b/src/main/deploy/pathplanner/paths/J Backup.path index 58e6778..bde7d8f 100644 --- a/src/main/deploy/pathplanner/paths/J Backup.path +++ b/src/main/deploy/pathplanner/paths/J Backup.path @@ -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 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/K Approach.path b/src/main/deploy/pathplanner/paths/K Approach.path new file mode 100644 index 0000000..96f11be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/K Approach.path @@ -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 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index dce0c13..51eab85 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -45,7 +45,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path b/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path index 74672e0..6ed2c94 100644 --- a/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path +++ b/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path @@ -41,7 +41,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Start to 30 Right.path b/src/main/deploy/pathplanner/paths/Start to 30 Right.path index e2e5358..da188f4 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -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, diff --git a/src/main/deploy/pathplanner/paths/Start to H.path b/src/main/deploy/pathplanner/paths/Start to H.path index 7585a51..711195e 100644 --- a/src/main/deploy/pathplanner/paths/Start to H.path +++ b/src/main/deploy/pathplanner/paths/Start to H.path @@ -41,7 +41,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/fein.path b/src/main/deploy/pathplanner/paths/fein.path index dc9b1fa..b1588cb 100644 --- a/src/main/deploy/pathplanner/paths/fein.path +++ b/src/main/deploy/pathplanner/paths/fein.path @@ -50,7 +50,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 47744e8..b86e264 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,7 +5,7 @@ "pathFolders": [], "autoFolders": [], "defaultMaxVel": 4.0, - "defaultMaxAccel": 1.0, + "defaultMaxAccel": 2.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 400.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 80277c2..15f1afb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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); diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 724e7f0..e3d29be 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -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; diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 11c7cc4..4763f0c 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -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 diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 18c5c84..8c028c9 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -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; + } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 4fdca3f..9e00608 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -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; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 96a5f04..0d4cf6e 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -161,7 +161,7 @@ public class Vision{ } public double getOrangeTimeStamp(){ - return orange_tx.getLastChange(); + return orange_tx.getLastChange()-VisionConstants.latencyFudge; } public boolean getOrangeTagDetected(){