diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto b/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto new file mode 100644 index 0000000..59ccb7c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto @@ -0,0 +1,74 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to 30 Right" + } + }, + { + "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": { + "name": "Collect Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "HP to 330 Left" + } + }, + { + "type": "named", + "data": { + "name": "Lift L4" + } + }, + { + "type": "path", + "data": { + "pathName": "K Approach" + } + }, + { + "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/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index fe904b3..9484f75 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.174795081967213, - "y": 7.321618852459016 + "x": 1.1987704918032787, + "y": 7.189754098360656 }, "prevControl": { - "x": 2.031259123063103, - "y": 6.375000701774084 + "x": 2.055234532899169, + "y": 6.243135947675724 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 cf9a6de..e85f75a 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/E to HP.path b/src/main/deploy/pathplanner/paths/E to HP.path index b0b204f..82d05e7 100644 --- a/src/main/deploy/pathplanner/paths/E to HP.path +++ b/src/main/deploy/pathplanner/paths/E to HP.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 1b8f1f1..8d9d773 100644 --- a/src/main/deploy/pathplanner/paths/H Backup.path +++ b/src/main/deploy/pathplanner/paths/H Backup.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 7136848..5c3fe60 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Left.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Left.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.174795081967213, - "y": 7.321618852459016 + "x": 1.1987704918032787, + "y": 7.189754098360656 }, "prevControl": null, "nextControl": { - "x": 2.4670039860768025, - "y": 7.096233578486413 + "x": 2.490979395912868, + "y": 6.964368824388053 }, "isLocked": false, "linkedName": "HP Left Position" @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to D.path b/src/main/deploy/pathplanner/paths/HP to D.path index 535ea23..9618c7f 100644 --- a/src/main/deploy/pathplanner/paths/HP to D.path +++ b/src/main/deploy/pathplanner/paths/HP to D.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 index 0caf4b8..7d7d20f 100644 --- a/src/main/deploy/pathplanner/paths/J Approach.path +++ b/src/main/deploy/pathplanner/paths/J Approach.path @@ -17,11 +17,11 @@ { "anchor": { "x": 4.974897540983607, - "y": 5.115881147540984 + "y": 5.235758196721312 }, "prevControl": { "x": 5.058811475409836, - "y": 5.403586065573771 + "y": 5.523463114754099 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/J Backup.path b/src/main/deploy/pathplanner/paths/J Backup.path index bde7d8f..3f52a71 100644 --- a/src/main/deploy/pathplanner/paths/J Backup.path +++ b/src/main/deploy/pathplanner/paths/J Backup.path @@ -4,12 +4,12 @@ { "anchor": { "x": 4.974897540983607, - "y": 5.115881147540984 + "y": 5.235758196721312 }, "prevControl": null, "nextControl": { "x": 5.11855393929902, - "y": 5.383513333352196 + "y": 5.503390382532524 }, "isLocked": false, "linkedName": "J" @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/K Approach.path b/src/main/deploy/pathplanner/paths/K Approach.path index 96f11be..8417384 100644 --- a/src/main/deploy/pathplanner/paths/K Approach.path +++ b/src/main/deploy/pathplanner/paths/K Approach.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.7970635805425386, - "y": 5.68234215716238 + "x": 3.6663590190101236, + "y": 5.6134008057257505 }, "isLocked": false, "linkedName": "Before K" }, { "anchor": { - "x": 3.8720286885245896, - "y": 5.043954918032787 + "x": 3.7290765411991873, + "y": 5.091647695859483 }, "prevControl": { - "x": 3.836065573770491, - "y": 5.295696721311475 + "x": 3.693113426445089, + "y": 5.343389499138171 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 51eab85..69ceee6 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -44,8 +44,8 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Right Start to E.path b/src/main/deploy/pathplanner/paths/Right Start to E.path index 99e8cc1..8ffb89f 100644 --- a/src/main/deploy/pathplanner/paths/Right Start to E.path +++ b/src/main/deploy/pathplanner/paths/Right Start to E.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 6.461044520547946, - "y": 0.478938356164384 + "y": 0.4789383561643841 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 da188f4..fb0742f 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -3,38 +3,50 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, - "y": 7.537397540983607 + "x": 7.1686475409836055, + "y": 7.573360655737705 }, "prevControl": null, "nextControl": { - "x": 6.3534836065573765, - "y": 6.51844262295082 + "x": 5.933913934426228, + "y": 6.5544057377049185 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.286577868852459, - "y": 5.955020491803278 + "x": 4.974897540983607, + "y": 5.235758196721312 }, "prevControl": { - "x": 5.872579581181226, - "y": 6.826510217830675 + "x": 5.560899253312374, + "y": 6.107247922748709 }, "nextControl": null, "isLocked": false, - "linkedName": "Before J" + "linkedName": "J" } ], "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Lift L4" + } + } + } + ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Start to H.path b/src/main/deploy/pathplanner/paths/Start to H.path index c238bec..b26d283 100644 --- a/src/main/deploy/pathplanner/paths/Start to H.path +++ b/src/main/deploy/pathplanner/paths/Start to H.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 b1588cb..90954a0 100644 --- a/src/main/deploy/pathplanner/paths/fein.path +++ b/src/main/deploy/pathplanner/paths/fein.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "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 b86e264..0ad5cef 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 2.5, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 1.0, "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 60bd03c..bf89433 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -126,9 +127,7 @@ public class RobotContainer { manipulator.setDefaultCommand( - manipulator.runUntilCollected( - () -> 0.0 - ) + manipulator.runManipulator(() -> 0.0, false) ); //Driver inputs @@ -160,19 +159,21 @@ public class RobotContainer { driver.start().whileTrue(drivetrain.resetToVision()); driver.rightBumper().whileTrue( + drivetrain.resetToVision().andThen( drivetrain.goToPose( () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], () -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180)) - ) + )) ); driver.leftBumper().whileTrue( + drivetrain.resetToVision().andThen( drivetrain.goToPose( () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], () -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180)) - ) + )) ); //Operator inputs @@ -209,7 +210,7 @@ public class RobotContainer { operator.start().toggleOnTrue(climberPivot.runPivot(() -> -operator.getRightY()*0.5).alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5))); operator.a().onTrue( - safeMoveManipulator(ElevatorConstants.kL1Position, ManipulatorPivotConstants.kStartingPosition) + safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition) ); operator.x().onTrue( @@ -233,13 +234,14 @@ public class RobotContainer { } private void configureNamedCommands() { - new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); - new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); + //new EventTrigger("Lift L4").onTrue(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", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runUntilCollected(() -> 0.0).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); - NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5)); - NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); + NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); + NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).until(() -> manipulator.getCoralBeamBreak() == false).andThen(manipulator.runManipulator(() -> 0, false)).withTimeout(0.1)); + NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position) + .andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1))); NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)); } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index e3d29be..41880bd 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -11,13 +11,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.trajectory.TrapezoidProfile; public class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 4; - public static final double kMaxAccelerationMetersPerSecondSquared = 2; + public static final double kMaxSpeedMetersPerSecond = 5.5; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 6; - public static final double kPYController = 6; + public static final double kPXController = 3; + public static final double kPYController = 3; public static final double kPThetaController = 5.5; // Constraint for the motion profiled robot angle controller diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index c09a4c3..65956aa 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -42,7 +42,7 @@ public class ElevatorConstants { public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kCoralIntakePosition = 0; - public static final double kL1Position = 0; + public static final double kL1Position = 14; public static final double kL2Position = 11; public static final double kL3Position = 27; public static final double kL4Position = 50.5; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index acf8d13..40e7953 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -38,7 +38,7 @@ public class ManipulatorPivotConstants { public static final double kStartingPosition = Units.degreesToRadians(90); public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90); - public static final double kL1Position = Units.degreesToRadians(0.0+90); + public static final double kL1Position = Units.degreesToRadians(246); public static final double kL2Position = Units.degreesToRadians(22.0+90); public static final double kL3Position = Units.degreesToRadians(22.0+90); public static final double kL4Position = Units.degreesToRadians(45.0+90); diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 763fb20..f20eaa2 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -59,6 +59,6 @@ public class VisionConstants { {4.993, 2.816, 5.272, 2.996} }; - public static final double latencyFudge = 0; + public static final double latencyFudge = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index e22cfdc..d2b1fc8 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -98,7 +98,6 @@ public class Drivetrain extends SubsystemBase { ); gyro = new AHRS(NavXComType.kMXP_SPI); - gyro.reset(); m_estimator = new SwerveDrivePoseEstimator( DrivetrainConstants.kDriveKinematics, @@ -110,7 +109,7 @@ public class Drivetrain extends SubsystemBase { m_rearRight.getPosition() }, new Pose2d(), - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)), VecBuilder.fill(1, 1, Units.degreesToRadians(360)) ); @@ -173,9 +172,9 @@ public class Drivetrain extends SubsystemBase { m_rearRight.getPosition() }); - gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue()); + gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){ @@ -197,7 +196,7 @@ public class Drivetrain extends SubsystemBase { if(vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ @@ -255,6 +254,7 @@ public class Drivetrain extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { + m_estimator.resetPosition( Rotation2d.fromDegrees(getGyroValue()), new SwerveModulePosition[] { @@ -451,7 +451,7 @@ public class Drivetrain extends SubsystemBase { } public Command resetToVision(){ - return run(() -> { + return runOnce(() -> { m_estimator.resetPose(orangePose2d); }); }