Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Enhanced odometry

Browse Source
This commit is contained in:
thenetworkgrinch
2023-04-08 13:14:18 -05:00
parent 5b38929c48
commit f5aabb594f
116 changed files with 2199 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
swervelib/SwerveDrive.java
View File

@@ -28,6 +28,7 @@ import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveKinematics2;
import swervelib.math.SwerveMath;
import swervelib.math.SwerveModuleState2;
import swervelib.math.SwervePoseEstimator2;
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.simulation.SwerveIMUSimulation;
@@ -51,7 +52,7 @@ public class SwerveDrive
/**
* Swerve odometry.
*/
public final SwerveDrivePoseEstimator swerveDrivePoseEstimator;
public final SwervePoseEstimator2 swerveDrivePoseEstimator;
/**
* Swerve modules.
*/
@@ -129,7 +130,7 @@ public class SwerveDrive
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator =
new SwerveDrivePoseEstimator(
new SwervePoseEstimator2(
kinematics,
getYaw(),
getModulePositions(),
@@ -685,7 +686,7 @@ public class SwerveDrive
public void updateOdometry()
{
// Update odometry
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
swerveDrivePoseEstimator.update(getYaw(), getPitch(), getRoll(), getModulePositions());
// Update angle accumulator if the robot is simulated
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())