Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix SwerveDriveKinematics not initializing a new array each time (#4942)

Browse Source 
This is problematic if you call it twice before utilizing the result.
This commit is contained in:
ohowe
2023-01-13 21:16:50 -07:00
committed by GitHub
parent 039edcc23f
commit e5a6197633
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -39,7 +39,7 @@ public class SwerveDriveKinematics {
private final int m_numModules;
private final Translation2d[] m_modules;
private final SwerveModuleState[] m_moduleStates;
private SwerveModuleState[] m_moduleStates;
private Translation2d m_prevCoR = new Translation2d();
/**
@@ -97,10 +97,12 @@ public class SwerveDriveKinematics {
if (chassisSpeeds.vxMetersPerSecond == 0.0
&& chassisSpeeds.vyMetersPerSecond == 0.0
&& chassisSpeeds.omegaRadiansPerSecond == 0.0) {
SwerveModuleState[] newStates = new SwerveModuleState[m_numModules];
for (int i = 0; i < m_numModules; i++) {
m_moduleStates[i].speedMetersPerSecond = 0.0;
newStates[i] = new SwerveModuleState(0.0, m_moduleStates[i].angle);
}
m_moduleStates = newStates;
return m_moduleStates;
}
@@ -132,6 +134,7 @@ public class SwerveDriveKinematics {
var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
m_moduleStates = new SwerveModuleState[m_numModules];
for (int i = 0; i < m_numModules; i++) {
double x = moduleStatesMatrix.get(i * 2, 0);
double y = moduleStatesMatrix.get(i * 2 + 1, 0);