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

Removed redundant lock function

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-14 22:49:55 -06:00
parent 11cd39b78f
commit 88cf3f8da3
89 changed files with 116 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
swervelib/SwerveDrive.java
View File

@@ -145,21 +145,6 @@ public class SwerveDrive
setModuleStates(swerveModuleStates, isOpenLoop);
}
/**
* Lock the swerve drive to prevent it from moving.
*/
public void lock()
{
double[] angles = new double[]{45, -45, -45, 45};
SwerveModuleState2[] moduleState2s = new SwerveModuleState2[swerveModules.length];
for (int i = 0; i < moduleState2s.length; i++)
{
swerveModules[i].lastAngle = angles[i % 4]; // Override jittering to ensure module is set to angle.
moduleState2s[i] = new SwerveModuleState2(0, Rotation2d.fromDegrees(angles[i % 4]), 0);
}
setModuleStates(moduleState2s, false);
}
/**
* Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.
*
@@ -337,7 +322,7 @@ public class SwerveDrive
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[1] : ypr[1]);
return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[2] : ypr[2]);
} else
{
return new Rotation2d();
@@ -357,9 +342,9 @@ public class SwerveDrive
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
return new Rotation3d(
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[0] : ypr[0]),
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[2] : ypr[2]),
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[1] : ypr[1]),
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[2] : ypr[2]));
Math.toRadians(swerveDriveConfiguration.invertedIMU ? 360 - ypr[0] : ypr[0]));
} else
{
return new Rotation3d(angle, 0, 0);