diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json index 95beffa..76b27b5 100644 --- a/.OutlineViewer/outlineviewer.json +++ b/.OutlineViewer/outlineviewer.json @@ -8,6 +8,12 @@ "NetworkTables Settings": { "mode": "Client (NT4)" }, + "client@1": { + "Publishers": { + "open": true + }, + "open": true + }, "client@2": { "open": true }, diff --git a/src/main/deploy/pathplanner/autos/fein.auto b/src/main/deploy/pathplanner/autos/fein.auto index dbd2a5c..0e47294 100644 --- a/src/main/deploy/pathplanner/autos/fein.auto +++ b/src/main/deploy/pathplanner/autos/fein.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "fein" + "pathName": "New New Path" } } ] diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path index 15c46cb..ea34786 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.5773565573770494, + "y": 6.506454918032786 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 3.5773565573770494, + "y": 6.506454918032786 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.891, - "y": 5.284 + "x": 4.735143442622951, + "y": 6.506454918032786 }, "prevControl": { - "x": 3.891, - "y": 5.284 + "x": 3.7351434426229506, + "y": 6.506454918032786 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 90.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ 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 a3dcaef..a58b6f7 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -179,7 +179,7 @@ public class Drivetrain extends SubsystemBase { if(vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -197,7 +197,7 @@ public class Drivetrain extends SubsystemBase { if(vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){ @@ -208,8 +208,9 @@ public class Drivetrain extends SubsystemBase { blackPose2d = vision.getBlackGlobalPose(gyroBuffer); m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); } - Logger.recordOutput("black pose", new Pose3d(blackPose2d)); } + Logger.recordOutput("black pose", new Pose3d(blackPose2d)); + Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition())); @@ -256,7 +257,13 @@ public class Drivetrain extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_estimator.resetPose( + m_estimator.resetPosition( + Rotation2d.fromDegrees(getGyroValue()), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition()}, pose ); } @@ -299,7 +306,7 @@ public class Drivetrain extends SubsystemBase { var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, - Rotation2d.fromDegrees(getGyroValue())) + new Rotation2d(m_estimator.getEstimatedPosition().getRotation().getRadians())) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index f715b1a..1f3ae87 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -106,7 +106,7 @@ public class Vision{ } public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer gyroBuffer){ - return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(), + return cameraToGlobalPose2d(getBlackClosestTag(), black_dist.get(), getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose); }