-
Notifications
You must be signed in to change notification settings - Fork 5
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge remote-tracking branch 'origin/main' into sysid-tuning
# Conflicts: # src/main/java/frc/robot/Constants.java
- Loading branch information
Showing
33 changed files
with
819 additions
and
511 deletions.
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,20 +1,18 @@ | ||
| | RPi Pico Pin | GP2040-CE | Button | Used For | | ||
| | ------------ | --------- | ------ | --------------------- | | ||
| | RPi Pico Pin | GP2040-CE | Button | Used For | | ||
| |-----------------|-----------|-----------|---------------------------------| | ||
| | **MODE SWITCH** | | ||
| | GP16 | S1 | BACK | Want cube(T)/cone(F)) | | ||
| | **MANUAL** | | ||
| | GP2 | UP | POV UP | Arm manual up | | ||
| | GP3 | DOWN | POV DOWN | Arm manual down | | ||
| | GP4 | RIGHT | POV RIGHT | Arm manual extend | | ||
| | GP5 | LEFT | POV LEFT | Arm manual retrack | | ||
| | **SCORE** | | ||
| | GP6 | B1 | A | Arm score low node | | ||
| | GP7 | B2 | B | Arm score mid-node | | ||
| | GP11 | B4 | Y | Arm score high node | | ||
| | **UTILITY** | | ||
| | GP10 | B3 | X | Arm home | | ||
| | GP18 | L3 | LS | Arm stop | | ||
| | GP19 | R3 | RS | Arm stop | | ||
| | **ACQUIRE** | | ||
| | GP13 | L1 | LB | Arm to shelf | | ||
| | GP12 | R1 | RB | Arm floor | | ||
| | GP16 | S1 | BACK | Solenoid Locked(f)/Unlocked(t) | | ||
| | **ARM** | | ||
| | GP2 | UP | POV UP | Arm manual up | | ||
| | GP3 | DOWN | POV DOWN | Arm manual down | | ||
| | **CLIMBER** | | ||
| | GP4 | RIGHT | POV RIGHT | Climber manual up | | ||
| | GP5 | LEFT | POV LEFT | Climber manual down | | ||
| | **SCORE** | | ||
| | GP6 | B1 | A | Spin Up Flywheel | | ||
| | GP7 | B2 | B | Expel intake and indexer | | ||
| | GP11 | B4 | Y | Spin Flywheel Backwards | | ||
| | **UTILITY** | | ||
| | GP10 | B3 | X | Arm home | | ||
| | GP13 | L1 | LB | Arm align to podium (hopefully) | | ||
| | GP12 | R1 | RB | Duck | |
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
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 |
|---|---|---|
| @@ -1,182 +1,37 @@ | ||
| package frc.lib.utils; | ||
|
|
||
| import edu.wpi.first.math.Matrix; | ||
| import edu.wpi.first.math.Nat; | ||
| import edu.wpi.first.math.VecBuilder; | ||
| import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; | ||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Twist2d; | ||
| import edu.wpi.first.math.numbers.N1; | ||
| import edu.wpi.first.math.numbers.N3; | ||
| import edu.wpi.first.wpilibj.Timer; | ||
| import java.util.ArrayList; | ||
| import java.util.Comparator; | ||
| import java.util.List; | ||
| import java.util.NavigableMap; | ||
| import java.util.TreeMap; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.kinematics.SwerveDriveKinematics; | ||
| import edu.wpi.first.math.kinematics.SwerveModulePosition; | ||
| import frc.robot.subsystems.drive.Drive; | ||
|
|
||
| public class RobotOdometry { | ||
| private static final double historyLengthSecs = 0.3; | ||
|
|
||
| private Pose2d basePose = new Pose2d(); | ||
| private Pose2d latestPose = new Pose2d(); | ||
| private final NavigableMap<Double, PoseUpdate> updates = new TreeMap<>(); | ||
| private final Matrix<N3, N1> q = new Matrix<>(Nat.N3(), Nat.N1()); | ||
|
|
||
| private static RobotOdometry instance = null; | ||
|
|
||
| /** | ||
| * Initializes the RobotOdometry instance with the provided state standard deviations. | ||
| * | ||
| * @param stateStdDevs a Matrix representing the standard deviations of the robot state in terms | ||
| * of the x, y, and theta coordinates | ||
| */ | ||
| public RobotOdometry(Matrix<N3, N1> stateStdDevs) { | ||
| for (int i = 0; i < 3; ++i) { | ||
| q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); | ||
| } | ||
| private static final RobotOdometry robotOdometry = new RobotOdometry(); | ||
| private SwerveDrivePoseEstimator estimator; | ||
| private SwerveModulePosition[] defaultPositions = | ||
| new SwerveModulePosition[] { | ||
| new SwerveModulePosition(), | ||
| new SwerveModulePosition(), | ||
| new SwerveModulePosition(), | ||
| new SwerveModulePosition() | ||
| }; | ||
|
|
||
| private RobotOdometry() { | ||
| estimator = | ||
| new SwerveDrivePoseEstimator( | ||
| new SwerveDriveKinematics(Drive.getModuleTranslations()), | ||
| new Rotation2d(), | ||
| defaultPositions, | ||
| new Pose2d()); | ||
| } | ||
|
|
||
| public static RobotOdometry getInstance() { | ||
| if (instance == null) { | ||
| instance = new RobotOdometry(VecBuilder.fill(0.003, 0.003, 0.0002)); | ||
| } | ||
| return instance; | ||
| } | ||
|
|
||
| /** Returns the latest robot pose based on drive and vision data. */ | ||
| public Pose2d getLatestPose() { | ||
| return latestPose; | ||
| } | ||
|
|
||
| /** Resets the odometry to a known pose. */ | ||
| public void resetPose(Pose2d pose) { | ||
| basePose = pose; | ||
| updates.clear(); | ||
| update(); | ||
| return robotOdometry; | ||
| } | ||
|
|
||
| /** Records a new drive movement. */ | ||
| public void addDriveData(double timestamp, Twist2d twist) { | ||
| updates.put(timestamp, new PoseUpdate(twist, new ArrayList<>())); | ||
| update(); | ||
| } | ||
|
|
||
| /** Records a new set of vision updates. */ | ||
| public void addVisionData(List<TimestampedVisionUpdate> visionData) { | ||
| for (var timestampedVisionUpdate : visionData) { | ||
| var timestamp = timestampedVisionUpdate.timestamp(); | ||
| var visionUpdate = | ||
| new VisionUpdate(timestampedVisionUpdate.pose(), timestampedVisionUpdate.stdDevs()); | ||
|
|
||
| if (updates.containsKey(timestamp)) { | ||
| // There was already an update at this timestamp, add to it | ||
| var oldVisionUpdates = updates.get(timestamp).visionUpdates(); | ||
| oldVisionUpdates.add(visionUpdate); | ||
| oldVisionUpdates.sort(VisionUpdate.compareDescStdDev); | ||
|
|
||
| } else { | ||
| // Insert a new update | ||
| var prevUpdate = updates.floorEntry(timestamp); | ||
| var nextUpdate = updates.ceilingEntry(timestamp); | ||
| if (prevUpdate == null || nextUpdate == null) { | ||
| // Outside the range of existing data | ||
| return; | ||
| } | ||
|
|
||
| // Create partial twists (prev -> vision, vision -> next) | ||
| var twist0 = | ||
| GeomUtils.multiplyTwist( | ||
| nextUpdate.getValue().twist(), | ||
| (timestamp - prevUpdate.getKey()) / (nextUpdate.getKey() - prevUpdate.getKey())); | ||
| var twist1 = | ||
| GeomUtils.multiplyTwist( | ||
| nextUpdate.getValue().twist(), | ||
| (nextUpdate.getKey() - timestamp) / (nextUpdate.getKey() - prevUpdate.getKey())); | ||
|
|
||
| // Add new pose updates | ||
| var newVisionUpdates = new ArrayList<VisionUpdate>(); | ||
| newVisionUpdates.add(visionUpdate); | ||
| newVisionUpdates.sort(VisionUpdate.compareDescStdDev); | ||
| updates.put(timestamp, new PoseUpdate(twist0, newVisionUpdates)); | ||
| updates.put( | ||
| nextUpdate.getKey(), new PoseUpdate(twist1, nextUpdate.getValue().visionUpdates())); | ||
| } | ||
| } | ||
|
|
||
| // Recalculate latest pose once | ||
| update(); | ||
| public SwerveDrivePoseEstimator getPoseEstimator() { | ||
| return estimator; | ||
| } | ||
|
|
||
| /** Clears old data and calculates the latest pose. */ | ||
| private void update() { | ||
| // Clear old data and update base pose | ||
| while (updates.size() > 1 | ||
| && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) { | ||
| var update = updates.pollFirstEntry(); | ||
| basePose = update.getValue().apply(basePose, q); | ||
| } | ||
|
|
||
| // Update latest pose | ||
| latestPose = basePose; | ||
| for (var updateEntry : updates.entrySet()) { | ||
| latestPose = updateEntry.getValue().apply(latestPose, q); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Represents a sequential update to a pose estimate, with a twist (drive movement) and list of | ||
| * vision updates. | ||
| */ | ||
| private record PoseUpdate(Twist2d twist, ArrayList<VisionUpdate> visionUpdates) { | ||
| public Pose2d apply(Pose2d lastPose, Matrix<N3, N1> q) { | ||
| // Apply drive twist | ||
| var pose = lastPose.exp(twist); | ||
|
|
||
| // Apply vision updates | ||
| for (var visionUpdate : visionUpdates) { | ||
| // Calculate Kalman gains based on std devs | ||
| // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) | ||
| Matrix<N3, N3> visionK = new Matrix<>(Nat.N3(), Nat.N3()); | ||
| var r = new double[3]; | ||
| for (int i = 0; i < 3; ++i) { | ||
| r[i] = visionUpdate.stdDevs().get(i, 0) * visionUpdate.stdDevs().get(i, 0); | ||
| } | ||
| for (int row = 0; row < 3; ++row) { | ||
| if (q.get(row, 0) == 0.0) { | ||
| visionK.set(row, row, 0.0); | ||
| } else { | ||
| visionK.set( | ||
| row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); | ||
| } | ||
| } | ||
|
|
||
| // Calculate twist between current and vision pose | ||
| var visionTwist = pose.log(visionUpdate.pose()); | ||
|
|
||
| // Multiply by Kalman gain matrix | ||
| var twistMatrix = | ||
| visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); | ||
|
|
||
| // Apply twist | ||
| pose = | ||
| pose.exp( | ||
| new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); | ||
| } | ||
|
|
||
| return pose; | ||
| } | ||
| } | ||
|
|
||
| /** Represents a single vision pose with associated standard deviations. */ | ||
| public record VisionUpdate(Pose2d pose, Matrix<N3, N1> stdDevs) { | ||
| public static final Comparator<VisionUpdate> compareDescStdDev = | ||
| (VisionUpdate a, VisionUpdate b) -> { | ||
| return -Double.compare( | ||
| a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), | ||
| b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); | ||
| }; | ||
| } | ||
|
|
||
| /** Represents a single vision pose with a timestamp and associated standard deviations. */ | ||
| public record TimestampedVisionUpdate(double timestamp, Pose2d pose, Matrix<N3, N1> stdDevs) {} | ||
| } |
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
Oops, something went wrong.