Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-29 07:01:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Addressing issue #7 by reading CANCoder values until successful with 10ms delay between readings. Fall back to reading relative encoder.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-20 20:59:31 -06:00
parent 8f9ffdf031
commit dd28a657b2
43 changed files with 570 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
swervelib/math/SwerveKinematics2.java
View File

@@ -76,8 +76,10 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
{
m_inverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
bigInverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
bigInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
bigInverseKinematics.setRow(
i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
bigInverseKinematics.setRow(
i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
}
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
@@ -197,11 +199,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
for (int i = 0; i < m_numModules; i++)
{
m_inverseKinematics.setRow(
i * 2,
0, /* Start Data */
1,
0,
-m_modules[i].getY() + centerOfRotationMeters.getY());
i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotationMeters.getY());
m_inverseKinematics.setRow(
i * 2 + 1,
0, /* Start Data */
@@ -237,14 +235,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
var accelerationVector = new SimpleMatrix(4, 1);
accelerationVector.setColumn(
0,
0,
0,
0,
Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2),
0
);
accelerationVector.setColumn(0, 0, 0, 0, Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), 0);
var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector);
@@ -260,26 +251,11 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
Rotation2d angle = new Rotation2d(x, y);
var trigThetaAngle = new SimpleMatrix(2, 2);
trigThetaAngle.setColumn(
0,
0,
angle.getCos(),
-angle.getSin()
);
trigThetaAngle.setColumn(
1,
0,
angle.getSin(),
angle.getCos()
);
trigThetaAngle.setColumn(0, 0, angle.getCos(), -angle.getSin());
trigThetaAngle.setColumn(1, 0, angle.getSin(), angle.getCos());
var accelVector = new SimpleMatrix(2, 1);
accelVector.setColumn(
0,
0,
ax,
ay
);
accelVector.setColumn(0, 0, ax, ay);
var omegaVector = trigThetaAngle.mult(accelVector);