Skip to content

Commit

Permalink
Address issue #159 and #166 by adding new PIDF helper functions, and …
Browse files Browse the repository at this point in the history
…optional anti jitter functions.

Signed-off-by: thenetworkgrinch <[email protected]>
  • Loading branch information
thenetworkgrinch committed Feb 28, 2024
1 parent 700eb54 commit 504e113
Show file tree
Hide file tree
Showing 2 changed files with 90 additions and 15 deletions.
16 changes: 8 additions & 8 deletions src/main/java/swervelib/SwerveDrive.java
Original file line number Diff line number Diff line change
Expand Up @@ -839,9 +839,9 @@ public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward
module.maxSpeed = maximumSpeed;
if (updateModuleFeedforward)
{
module.feedforward = SwerveMath.createDriveFeedforward(optimalVoltage,
maximumSpeed,
swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction);
module.setFeedforward(SwerveMath.createDriveFeedforward(optimalVoltage,
maximumSpeed,
swerveDriveConfiguration.physicalCharacteristics.wheelGripCoefficientOfFriction));
}
}
}
Expand All @@ -851,7 +851,7 @@ public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and
* {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
* what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the
* {@link SwerveModule#feedforward}.
* {@link SwerveModule#setFeedforward(SimpleMotorFeedforward)}.
*
* @param maximumSpeed Maximum speed for the drive motors in meters / second.
*/
Expand Down Expand Up @@ -908,13 +908,13 @@ public Pose2d[] getSwerveModulePoses(Pose2d robotPose)
/**
* Setup the swerve module feedforward.
*
* @param feedforward Feedforward for the drive motor on swerve modules.
* @param driveFeedforward Feedforward for the drive motor on swerve modules.
*/
public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward)
public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforward)
{
for (SwerveModule swerveModule : swerveModules)
{
swerveModule.feedforward = feedforward;
swerveModule.setFeedforward(driveFeedforward);
}
}

Expand Down Expand Up @@ -1114,7 +1114,7 @@ public void resetDriveEncoders()
{
for (SwerveModule module : swerveModules)
{
module.configuration.driveMotor.setPosition(0);
module.getDriveMotor().setPosition(0);
}
}

Expand Down
89 changes: 82 additions & 7 deletions src/main/java/swervelib/SwerveModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import swervelib.math.SwerveMath;
import swervelib.motors.SwerveMotor;
import swervelib.parser.Cache;
import swervelib.parser.PIDFConfig;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.simulation.SwerveModuleSimulation;
import swervelib.telemetry.Alert;
Expand Down Expand Up @@ -78,13 +79,17 @@ public class SwerveModule
*/
public int moduleNumber;
/**
* Feedforward for drive motor during closed loop control.
* Feedforward for the drive motor during closed loop control.
*/
public SimpleMotorFeedforward feedforward;
private SimpleMotorFeedforward driveMotorFeedforward;
/**
* Maximum speed of the drive motors in meters per second.
*/
public double maxSpeed;
/**
* Anti-Jitter AKA auto-centering disabled.
*/
private boolean antiJitterEnabled = true;
/**
* Last swerve module state applied.
*/
Expand Down Expand Up @@ -122,8 +127,8 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat
configuration = moduleConfiguration;
angleOffset = moduleConfiguration.angleOffset;

// Initialize Feedforward for drive motor.
feedforward = driveFeedforward;
// Initialize Feedforwards.
driveMotorFeedforward = driveFeedforward;

// Create motors from configuration and reset them to defaults.
angleMotor = moduleConfiguration.angleMotor;
Expand Down Expand Up @@ -236,6 +241,76 @@ public void queueSynchronizeEncoders()
}
}

/**
* Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor
* controllers as well.
*
* @param antiJitter Anti-Jitter state desired.
*/
public void setAntiJitter(boolean antiJitter)
{
this.antiJitterEnabled = antiJitter;
if (antiJitter)
{
pushOffsetsToControllers();
} else
{
restoreInternalOffset();
}
}

/**
* Set the feedforward attributes to the given parameters.
*
* @param drive Drive motor feedforward for the module.
*/
public void setFeedforward(SimpleMotorFeedforward drive)
{
this.driveMotorFeedforward = drive;
}

/**
* Set the drive PIDF values.
*
* @param config {@link PIDFConfig} of that should be set.
*/
public void setDrivePIDF(PIDFConfig config)
{
configuration.velocityPIDF = config;
driveMotor.configurePIDF(config);
}

/**
* Get the current drive motor PIDF values.
*
* @return {@link PIDFConfig} of the drive motor.
*/
public PIDFConfig getDrivePIDF()
{
return configuration.velocityPIDF;
}

/**
* Set the angle/azimuth/steering motor PID
*
* @param config {@link PIDFConfig} of that should be set.
*/
public void setAnglePIDF(PIDFConfig config)
{
configuration.anglePIDF = config;
angleMotor.configurePIDF(config);
}

/**
* Get the current angle/azimuth/steering motor PIDF values.
*
* @return {@link PIDFConfig} of the angle motor.
*/
public PIDFConfig getAnglePIDF()
{
return configuration.anglePIDF;
}

/**
* 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>
Expand All @@ -250,7 +325,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));

// If we are forcing the angle
if (!force)
if (!force && antiJitterEnabled)
{
// Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
Expand All @@ -267,7 +342,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
driveMotor.set(percentOutput);
} else
{
driveMotor.setReference(velocity, feedforward.calculate(velocity));
driveMotor.setReference(velocity, driveMotorFeedforward.calculate(velocity));
desiredState.speedMetersPerSecond = velocity;
}

Expand Down Expand Up @@ -507,7 +582,7 @@ public SwerveModuleConfiguration getConfiguration()
*/
public void pushOffsetsToControllers()
{
if (absoluteEncoder != null)
if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
{
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
{
Expand Down

0 comments on commit 504e113

Please sign in to comment.