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

Updated and added absolute encoder stability

Browse Source
This commit is contained in:
thenetworkgrinch
2023-03-15 21:49:24 -05:00
parent e15e538d56
commit 9887cf3c26
117 changed files with 1527 additions and 1114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
swervelib/SwerveModule.java
View File

@@ -55,6 +55,10 @@ public class SwerveModule
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
@@ -95,7 +99,7 @@ public class SwerveModule
{
absoluteEncoder.factoryDefault();
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
angleMotor.setPosition(getAbsolutePosition() - angleOffset);
angleMotor.setPosition(getAbsolutePosition());
}
// Config angle motor/controller
@@ -125,18 +129,19 @@ public class SwerveModule
}
/**
* Synchronize the integrated angle encoder with the absolute encoder.
* Queue synchronization of the integrated angle encoder with the absolute encoder.
*/
public void synchronizeEncoders()
public void queueSynchronizeEncoders()
{
if (absoluteEncoder != null)
{
angleMotor.setPosition(getAbsolutePosition() - angleOffset);
synchronizeEncoderQueued = true;
}
}
/**
* Set the desired state of the swerve module.
* Set the desired state of the swerve module. <br /><b>WARNING: If you are not using one of the functions from
* {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics}</b>
*
* @param desiredState Desired swerve module state.
* @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
@@ -186,10 +191,22 @@ public class SwerveModule
// Prevents module rotation if speed is less than 1%
angle = Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ? lastAngle : angle;
// Prevent module rotation if angle is the same as the previous angle.
if (angle != lastAngle)
if (angle != lastAngle || synchronizeEncoderQueued)
{
angleMotor.setReference(
angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
if (absoluteEncoder != null && synchronizeEncoderQueued)
{
double absoluteEncoderPosition = getAbsolutePosition();
angleMotor.setPosition(absoluteEncoderPosition);
angleMotor.setReference(angle,
Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV,
absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
{
angleMotor.setReference(
angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
}
}
}
lastAngle = angle;
@@ -260,22 +277,29 @@ public class SwerveModule
/**
* Get the absolute position. Falls back to relative position on reading failure.
*
* @return Absolute encoder angle in degrees.
* @return Absolute encoder angle in degrees in the range [0, 360).
*/
public double getAbsolutePosition()
{
double angle;
if (absoluteEncoder != null)
{
double angle = absoluteEncoder.getAbsolutePosition();
angle = absoluteEncoder.getAbsolutePosition() - angleOffset;
if (absoluteEncoder.readingError)
{
angle = getRelativePosition();
}
return angle;
} else
{
angle = getRelativePosition();
}
angle %= 360;
if (angle < 0.0)
{
angle += 360;
}
return getRelativePosition();
return angle;
}
/**