Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix MecanumDriveKinematics (#3266)

Browse Source
This commit is contained in:
Tyler Veness
2021-04-30 15:50:16 -07:00
committed by GitHub
parent ff52f207cc
commit 365f5449ca
6 changed files with 84 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
View File

@@ -30,7 +30,7 @@ import org.ejml.simple.SimpleMatrix;
* field using encoders and a gyro.
*/
public class MecanumDriveKinematics {
private SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
private final Translation2d m_frontLeftWheelMeters;
@@ -167,6 +167,5 @@ public class MecanumDriveKinematics {
m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
}
}

1
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -62,5 +62,4 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
1, 1, (rl.X() - rl.Y()).template to<double>(),
1, -1, (-(rr.X() + rr.Y())).template to<double>();
// clang-format on
m_inverseKinematics /= std::sqrt(2);
}