Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
starting 6328-style state estimation
- Loading branch information
1 parent
7c03b2d
commit e62e16b
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,61 @@ | ||
| package frc.robot; | ||
|
|
||
| import edu.wpi.first.math.Matrix; | ||
| import edu.wpi.first.math.VecBuilder; | ||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Twist2d; | ||
| import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; | ||
| import edu.wpi.first.math.kinematics.SwerveDriveKinematics; | ||
| import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; | ||
| import edu.wpi.first.math.kinematics.SwerveModulePosition; | ||
| import edu.wpi.first.math.numbers.N1; | ||
| import edu.wpi.first.math.numbers.N3; | ||
|
|
||
| public class RobotState { | ||
|
Check warning on line 15 in src/main/java/frc/robot/RobotState.java
|
||
|
|
||
| public record OdometryObservation(SwerveDriveWheelPositions wheelPositions, | ||
|
Check warning on line 17 in src/main/java/frc/robot/RobotState.java
|
||
| Rotation2d gyroAngle, double timestamp) { | ||
| } | ||
|
|
||
| public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) { | ||
|
Check warning on line 21 in src/main/java/frc/robot/RobotState.java
|
||
| } | ||
|
|
||
| private static final double poseBufferSizeSeconds = 2.0; | ||
|
|
||
| private static RobotState instance = new RobotState(); | ||
|
|
||
| public static RobotState getInstance() { | ||
| return instance; | ||
| } | ||
|
|
||
| private Pose2d odometryPose = new Pose2d(); | ||
| private Pose2d estimatedPose = new Pose2d(); | ||
| private final TimeInterpolatableBuffer<Pose2d> poseBuffer = | ||
| TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds); | ||
| private final Matrix<N3, N1> qStdDevs = | ||
| new Matrix<>(VecBuilder.fill(Math.pow(0.003, 2), Math.pow(0.003, 2), Math.pow(0.0002, 2))); | ||
| // Odometry | ||
| private final SwerveDriveKinematics kinematics; | ||
| private SwerveDriveWheelPositions lastWheelPositions = | ||
| new SwerveDriveWheelPositions(new SwerveModulePosition[] {new SwerveModulePosition(), | ||
| new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition()}); | ||
| private Rotation2d lastGyroAngle = new Rotation2d(); | ||
| private Twist2d robotVelocity = new Twist2d(); | ||
| private Twist2d trajectoryVelocity = new Twist2d(); | ||
|
|
||
| private RobotState() { | ||
| this.kinematics = Constants.Swerve.swerveKinematics; | ||
| } | ||
|
|
||
| public void addOdometryObservation(OdometryObservation observation) { | ||
| // TODO | ||
| // https://github.com/Mechanical-Advantage/RobotCode2024/blob/a025615a52193b7709db7cf14c51c57be17826f2/src/main/java/org/littletonrobotics/frc2024/RobotState.java#L160 | ||
| } | ||
|
|
||
| public void addVisionObservation(VisionObservation observation) { | ||
| // TODO | ||
| // https://github.com/Mechanical-Advantage/RobotCode2024/blob/a025615a52193b7709db7cf14c51c57be17826f2/src/main/java/org/littletonrobotics/frc2024/RobotState.java#L181 | ||
| } | ||
|
|
||
| } | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,28 @@ | ||
| package frc.robot.subsystems.vision; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.lib.util.photon.PhotonCameraWrapper; | ||
| import frc.robot.Constants; | ||
|
|
||
| public class Vision extends SubsystemBase { | ||
|
Check warning on line 7 in src/main/java/frc/robot/subsystems/vision/Vision.java
|
||
|
|
||
| private final PhotonCameraWrapper[] cameras; | ||
|
|
||
| public Vision(boolean isSim) { | ||
|
Check warning on line 11 in src/main/java/frc/robot/subsystems/vision/Vision.java
|
||
| super("Vision"); | ||
| cameras = new PhotonCameraWrapper[] { | ||
| new PhotonCameraWrapper(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, | ||
| Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP, | ||
| Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT)}; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| for (var cam : cameras) { | ||
| cam.periodic(); | ||
| } | ||
| // TODO | ||
| // https://github.com/Mechanical-Advantage/RobotCode2024/blob/a025615a52193b7709db7cf14c51c57be17826f2/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java#L59 | ||
| } | ||
|
|
||
| } | ||