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 ccfa902..54def77 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.0549180327868852, - "y": 7.285655737704918 + "x": 1.2587090163934425, + "y": 7.08186475409836 }, "prevControl": { - "x": 1.9113820738827756, - "y": 6.339037587019986 + "x": 2.115173057489333, + "y": 6.135246603413428 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 2.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Right.path b/src/main/deploy/pathplanner/paths/HP to 330 Right.path index 6b76674..096023c 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Right.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Right.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0549180327868852, - "y": 7.285655737704918 + "x": 1.2587090163934425, + "y": 7.08186475409836 }, "prevControl": null, "nextControl": { - "x": 2.3471269368964744, - "y": 7.060270463732315 + "x": 2.550917920503032, + "y": 6.856479480125757 }, "isLocked": false, "linkedName": "HP Left Position" }, { "anchor": { - "x": 3.5723360655737704, - "y": 4.996004098360656 + "x": 3.6442622950819668, + "y": 5.031967213114754 }, "prevControl": { - "x": 3.256796682012127, - "y": 5.521903070963395 + "x": 3.328722911520323, + "y": 5.557866185717494 }, "nextControl": null, "isLocked": false, @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.25, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path index 5b5e435..9d72bd4 100644 --- a/src/main/deploy/pathplanner/paths/HP to K.path +++ b/src/main/deploy/pathplanner/paths/HP to K.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.0549180327868852, - "y": 7.285655737704918 + "x": 1.2587090163934425, + "y": 7.08186475409836 }, "prevControl": null, "nextControl": { - "x": 1.5703893442622952, - "y": 6.734221311475409 + "x": 1.7741803278688524, + "y": 6.530430327868851 }, "isLocked": false, "linkedName": "HP Left Position" diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index 4d0efd8..2c7a178 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5723360655737704, - "y": 4.996004098360656 + "x": 3.6442622950819668, + "y": 5.031967213114754 }, "prevControl": null, "nextControl": { - "x": 3.4398363305729753, - "y": 5.208003674361928 + "x": 3.5117625600811717, + "y": 5.243966789116026 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 1.0549180327868852, - "y": 7.285655737704918 + "x": 1.2587090163934425, + "y": 7.08186475409836 }, "prevControl": { - "x": 1.258709016393443, - "y": 7.009938524590164 + "x": 1.4625000000000001, + "y": 6.8061475409836065 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index deca0d1..2124be5 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -180,14 +180,16 @@ public class Drivetrain extends SubsystemBase { m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ - if(vision.getOrangeDist() < 60 && getVelocity() < 2){ + if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); - }else if(vision.getOrangeDist() < 100 && getVelocity() < 2){ + }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); - }else if(vision.getOrangeDist() < 60 && getVelocity() > 2){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); - }else if(vision.getOrangeDist() < 100 && getVelocity() > 2){ + }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); + }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); + }else if(getVelocity() < 1.5){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.06, 0.06, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -206,14 +208,16 @@ public class Drivetrain extends SubsystemBase { if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ - if(vision.getBlackDist() < 60 && getVelocity() < 2){ + if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); - }else if(vision.getBlackDist() < 100 && getVelocity() < 2){ + }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); - }else if(vision.getBlackDist() < 60 && getVelocity() > 2){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); - }else if(vision.getBlackDist() < 100 && getVelocity() > 2){ + }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); + }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); + }else if(getVelocity() < 1.5){ + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.06, 0.06, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){