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 34c74e8..9d44a71 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.1987704918032787, - "y": 7.189754098360656 + "x": 1.0549180327868852, + "y": 7.285655737704918 }, "prevControl": { - "x": 2.055234532899169, - "y": 6.243135947675724 + "x": 1.9113820738827756, + "y": 6.339037587019986 }, "nextControl": null, "isLocked": false, 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 7ee14ee..71bb973 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.1987704918032787, - "y": 7.189754098360656 + "x": 1.0549180327868852, + "y": 7.285655737704918 }, "prevControl": null, "nextControl": { - "x": 2.490979395912868, - "y": 6.964368824388053 + "x": 2.3471269368964744, + "y": 7.060270463732315 }, "isLocked": false, "linkedName": "HP Left Position" }, { "anchor": { - "x": 3.6202868852459016, - "y": 5.031967213114754 + "x": 3.65625, + "y": 5.05594262295082 }, "prevControl": { - "x": 3.304747501684258, - "y": 5.557866185717494 + "x": 3.3407106164383564, + "y": 5.581841595553559 }, "nextControl": null, "isLocked": false, @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.75, + "maxAcceleration": 1.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -62,5 +62,5 @@ "velocity": 0, "rotation": -53.97262661489646 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path index 3f834b0..5b5e435 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.1987704918032787, - "y": 7.189754098360656 + "x": 1.0549180327868852, + "y": 7.285655737704918 }, "prevControl": null, "nextControl": { - "x": 1.7142418032786886, - "y": 6.638319672131147 + "x": 1.5703893442622952, + "y": 6.734221311475409 }, "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 53135f0..fb45c41 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.6202868852459016, - "y": 5.031967213114754 + "x": 3.65625, + "y": 5.05594262295082 }, "prevControl": null, "nextControl": { - "x": 3.4877871502451065, - "y": 5.243966789116026 + "x": 3.523750264999205, + "y": 5.267942198952092 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 1.1987704918032787, - "y": 7.189754098360656 + "x": 1.0549180327868852, + "y": 7.285655737704918 }, "prevControl": { - "x": 1.4025614754098363, - "y": 6.914036885245903 + "x": 1.258709016393443, + "y": 7.009938524590164 }, "nextControl": null, "isLocked": false, 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 70e06de..a5b20af 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 1.75, + "maxAcceleration": 1.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -62,5 +62,5 @@ "velocity": 0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e5c4693..3cc0d61 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -112,7 +112,7 @@ public class Drivetrain extends SubsystemBase { }, new Pose2d(), VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)), - VecBuilder.fill(1, 1, Units.degreesToRadians(360)) + VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)) ); pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints); @@ -177,11 +177,11 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.10, 0.10, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -199,7 +199,7 @@ public class Drivetrain extends SubsystemBase { if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index c5baba4..d223707 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -84,8 +84,8 @@ public class Manipulator extends SubsystemBase { */ public Command runUntilCollected(DoubleSupplier speed) { return run(() -> { - manipulatorMotor.set( - speed.getAsDouble() + manipulatorMotor.setVoltage( + speed.getAsDouble()*12 ); indexerMotor.set(1);