Skip to content

Commit

Permalink
Reformatted code, generated javadocs.
Browse files Browse the repository at this point in the history
Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
  • Loading branch information
thenetworkgrinch committed Dec 12, 2023
1 parent ade674f commit 68ecbe9
Show file tree
Hide file tree
Showing 11 changed files with 84 additions and 77 deletions.
39 changes: 20 additions & 19 deletions src/main/java/swervelib/SwerveDrive.java
Original file line number Diff line number Diff line change
Expand Up @@ -66,11 +66,11 @@ public class SwerveDrive
/**
* Odometry lock to ensure thread safety.
*/
private final Lock odometryLock = new ReentrantLock();
private final Lock odometryLock = new ReentrantLock();
/**
* Field object.
*/
public Field2d field = new Field2d();
public Field2d field = new Field2d();
/**
* Swerve controller for controlling heading of the robot.
*/
Expand All @@ -79,31 +79,31 @@ public class SwerveDrive
* Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of
* rotation)
*/
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1,
0.1,
0.1);
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1,
0.1,
0.1);
/**
* The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more
* points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)}
* with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get
* vision accurate to inches instead of feet.
*/
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9,
0.9,
0.9);
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9,
0.9,
0.9);
/**
* Invert odometry readings of drive motor positions, used as a patch for debugging currently.
*/
public boolean invertOdometry = false;
public boolean invertOdometry = false;
/**
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
*/
public boolean chassisVelocityCorrection = true;
public boolean chassisVelocityCorrection = true;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
public boolean headingCorrection = false;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
Expand All @@ -115,23 +115,23 @@ public class SwerveDrive
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
private double lastHeadingRadians = 0;
/**
* The absolute max speed that your robot can reach while translating in meters per second.
*/
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
/**
* The absolute max speed the robot can reach while rotating radians per second.
*/
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
/**
* Maximum speed of the robot in meters per second.
*/
private double maxSpeedMPS;
private double maxSpeedMPS;

/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
Expand Down Expand Up @@ -902,7 +902,8 @@ public void updateOdometry()
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition());
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Absolute Encoder Read Issue", module.getAbsoluteEncoderReadIssue() ? 1 : 0);
"Module[" + module.configuration.name + "] Absolute Encoder Read Issue",
module.getAbsoluteEncoderReadIssue() ? 1 : 0);
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
Expand Down Expand Up @@ -1072,8 +1073,8 @@ public void setModuleStateOptimization(boolean optimizationEnabled)
}

/**
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type.
* Also removes the internal offsets to prevent double offsetting.
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
* internal offsets to prevent double offsetting.
*/
public void pushOffsetsToControllers()
{
Expand Down
51 changes: 28 additions & 23 deletions src/main/java/swervelib/SwerveModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,6 @@ public class SwerveModule
* Swerve module configuration options.
*/
public final SwerveModuleConfiguration configuration;
/**
* Angle offset from the absolute encoder.
*/
private double angleOffset;
/**
* Swerve Motors.
*/
Expand All @@ -39,32 +35,36 @@ public class SwerveModule
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
public int moduleNumber;
public int moduleNumber;
/**
* Feedforward for drive motor during closed loop control.
*/
public SimpleMotorFeedforward feedforward;
public SimpleMotorFeedforward feedforward;
/**
* Maximum speed of the drive motors in meters per second.
*/
public double maxSpeed;
public double maxSpeed;
/**
* Last swerve module state applied.
*/
public SwerveModuleState lastState;
public SwerveModuleState lastState;
/**
* Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module
* never turns more than 90deg.
*/
public boolean moduleStateOptimization = true;
public boolean moduleStateOptimization = true;
/**
* Angle offset from the absolute encoder.
*/
private double angleOffset;
/**
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
private SwerveModuleSimulation simModule;
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;

/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
Expand Down Expand Up @@ -390,22 +390,23 @@ public SwerveModuleConfiguration getConfiguration()
}

/**
* Push absolute encoder offset in the memory of the encoder or controller.
* Also removes the internal angle offset.
* Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset.
*/
public void pushOffsetsToControllers()
{
if(absoluteEncoder != null)
if (absoluteEncoder != null)
{
if(absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)){
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
{
angleOffset = 0;
} else
{
DriverStation.reportWarning(
"Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false);
}
else{
DriverStation.reportWarning("Pushing the Absolute Encoder offset to the encoder failed on module #"+moduleNumber, false);
}
}
else{
DriverStation.reportWarning("There is no Absolute Encoder on module #"+moduleNumber, false);
} else
{
DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false);
}
}

Expand All @@ -417,16 +418,20 @@ public void restoreInternalOffset()
absoluteEncoder.setAbsoluteEncoderOffset(0);
angleOffset = configuration.angleOffset;
}

/**
* Get if the last Absolute Encoder had a read issue, such as it does not exist.
*
* @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist.
*/
public boolean getAbsoluteEncoderReadIssue()
{
if(absoluteEncoder == null)
if (absoluteEncoder == null)
{
return true;
else
} else
{
return absoluteEncoder.readingError;
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.DriverStation;

/**
* Swerve Absolute Encoder for Thrifty Encoders and other analog encoders.
Expand Down Expand Up @@ -94,19 +93,19 @@ public Object getAbsoluteEncoder()

/**
* Cannot Set the offset of an Analog Absolute Encoder.
*
*
* @param offset the offset the Absolute Encoder uses as the zero point.
*
* @return Will always be false as setting the offset is unsupported of an Analog absolute encoder.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
//Do Nothing
DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of Analog Encoders Channel #"+encoder.getChannel(), false);
DriverStation.reportWarning(
"Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false);
return false;
}
}

/**
* Get the velocity in degrees/sec.
*
Expand All @@ -115,7 +114,7 @@ public boolean setAbsoluteEncoderOffset(double offset)
@Override
public double getVelocity()
{
DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!",true);
DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true);
return encoder.getValue();
}
}
12 changes: 6 additions & 6 deletions src/main/java/swervelib/encoders/CANCoderSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -140,23 +140,23 @@ public Object getAbsoluteEncoder()

/**
* Sets the Absolute Encoder Offset inside of the CANcoder's Memory.
*
*
* @param offset the offset the Absolute Encoder uses as the zero point.
*
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
ErrorCode error = encoder.configMagnetOffset(offset);
if(error == ErrorCode.OK)
if (error == ErrorCode.OK)
{
return true;
}
DriverStation.reportWarning("Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: "+error, false);
DriverStation.reportWarning(
"Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false);
return false;
}
}

/**
* Get the velocity in degrees/sec.
*
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/swervelib/encoders/CanAndCoderSwerve.java
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package swervelib.encoders;

import com.reduxrobotics.sensors.canandcoder.CANandcoder;

import edu.wpi.first.wpilibj.DriverStation;

/**
Expand Down Expand Up @@ -82,19 +81,19 @@ public Object getAbsoluteEncoder()

/**
* Cannot set the offset of the CanAndCoder.
*
*
* @param offset the offset the Absolute Encoder uses as the zero point.
*
* @return always false due to CanAndCoder not supporting offset changing.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
//CanAndCoder does not support Absolute Offset Changing
DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: "+encoder.getAddress(), false);
DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(),
false);
return false;
}

/**
* Get the velocity in degrees/sec.
*
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -98,9 +98,8 @@ public void clearStickyFaults()

/**
* Sets the offset of the Encoder in the WPILib Encoder Library.
*
*
* @param offset the offset the Absolute Encoder uses as the zero point.
*
* @return Always true due to no external device commands.
*/
@Override
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -108,27 +108,27 @@ public Object getAbsoluteEncoder()

/**
* Sets the Absolute Encoder Offset inside of the SparkMax's Memory.
*
*
* @param offset the offset the Absolute Encoder uses as the zero point.
*
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
REVLibError error = null;
for(int i=0; i < maximumRetries; i++)
for (int i = 0; i < maximumRetries; i++)
{
error = encoder.setZeroOffset(offset);
if(error == REVLibError.kOk)
if (error == REVLibError.kOk)
{
return true;
}
}
DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: "+ error, false);
DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false);
return false;
}
/**

/**
* Get the velocity in degrees/sec.
*
* @return velocity in degrees/sec.
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,13 @@ public abstract class SwerveAbsoluteEncoder

/**
* Sets the Absolute Encoder offset at the Encoder Level.
*
*
* @param offset the offset the Absolute Encoder uses as the zero point.
*
* @return if setting Absolute Encoder Offset was successful or not.
*/
public abstract boolean setAbsoluteEncoderOffset(double offset);
/*

/**
* Get the velocity in degrees/sec.
* @return velocity in degrees/sec.
*/
Expand Down
Loading

0 comments on commit 68ecbe9

Please sign in to comment.