Skip to content

Commit

Permalink
Updated formatting.
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 Jan 17, 2024
1 parent 7ad0c77 commit a63d238
Show file tree
Hide file tree
Showing 18 changed files with 341 additions and 260 deletions.
22 changes: 11 additions & 11 deletions src/main/java/swervelib/SwerveDrive.java
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,10 @@ public class SwerveDrive
* Odometry lock to ensure thread safety.
*/
private final Lock odometryLock = new ReentrantLock();
/**
* Deadband for speeds in heading correction.
*/
private final double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Field object.
*/
Expand Down Expand Up @@ -99,14 +103,14 @@ public class SwerveDrive
* correction.
*/
public boolean chassisVelocityCorrection = true;
/**
* Whether heading correction PID is currently active.
*/
private boolean correctionEnabled = false;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
/**
* Whether heading correction PID is currently active.
*/
private boolean correctionEnabled = false;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
Expand All @@ -119,10 +123,6 @@ public class SwerveDrive
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
/**
* Deadband for speeds in heading correction.
*/
private final double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* The last heading set in radians.
*/
Expand Down Expand Up @@ -412,10 +412,10 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
if (headingCorrection)
{
if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
if (!correctionEnabled)
if (!correctionEnabled)
{
lastHeadingRadians = getYaw().getRadians();
correctionEnabled = true;
Expand Down
16 changes: 12 additions & 4 deletions src/main/java/swervelib/SwerveModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -64,11 +64,17 @@ public class SwerveModule
/**
* An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails.
*/
private Alert encoderOffsetWarning = new Alert("Motors", "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, Alert.AlertType.WARNING);
private Alert encoderOffsetWarning = new Alert("Motors",
"Pushing the Absolute Encoder offset to the encoder failed on module #" +
moduleNumber,
Alert.AlertType.WARNING);
/**
* An {@link Alert} for if there is no Absolute Encoder on the module.
*/
private Alert noEncoderWarning = new Alert("Motors", "There is no Absolute Encoder on module #" + moduleNumber, Alert.AlertType.WARNING);
private Alert noEncoderWarning = new Alert("Motors",
"There is no Absolute Encoder on module #" +
moduleNumber,
Alert.AlertType.WARNING);

/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
Expand Down Expand Up @@ -405,10 +411,12 @@ public void pushOffsetsToControllers()
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
{
angleOffset = 0;
} else {
} else
{
encoderOffsetWarning.set(true);
}
} else {
} else
{
noEncoderWarning.set(true);
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,18 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
/**
* Inversion state of the encoder.
*/
private boolean inverted = false;
/** An {@link Alert} for if the absolute encoder offset cannot be set. */
private Alert cannotSetOffset = new Alert(
private boolean inverted = false;
/**
* An {@link Alert} for if the absolute encoder offset cannot be set.
*/
private Alert cannotSetOffset = new Alert(
"Encoders",
"Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(),
Alert.AlertType.WARNING);
/** An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. */
private Alert inaccurateVelocities = new Alert(
/**
* An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities.
*/
private Alert inaccurateVelocities = new Alert(
"Encoders",
"The Analog Absolute encoder may not report accurate velocities!",
Alert.AlertType.WARNING_TRACE);
Expand Down
51 changes: 30 additions & 21 deletions src/main/java/swervelib/encoders/CANCoderSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,28 +20,36 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* CANCoder with WPILib sendable and support.
*/
public CANcoder encoder;
/** An {@link Alert} for if the CANCoder magnet field is less than ideal. */
private Alert magnetFieldLessThanIdeal = new Alert(
public CANcoder encoder;
/**
* An {@link Alert} for if the CANCoder magnet field is less than ideal.
*/
private Alert magnetFieldLessThanIdeal = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.",
Alert.AlertType.WARNING);
/** An {@link Alert} for if the CANCoder reading is faulty. */
private Alert readingFaulty = new Alert(
/**
* An {@link Alert} for if the CANCoder reading is faulty.
*/
private Alert readingFaulty = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " reading was faulty.",
Alert.AlertType.WARNING);
/** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */
private Alert readingIgnored = new Alert(
/**
* An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored.
*/
private Alert readingIgnored = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.",
Alert.AlertType.WARNING);
/** An {@link Alert} for if the absolute encoder offset cannot be set. */
private Alert cannotSetOffset = new Alert(
/**
* An {@link Alert} for if the absolute encoder offset cannot be set.
*/
private Alert cannotSetOffset = new Alert(
"Encoders",
"Failure to set CANCoder "
+ encoder.getDeviceID()
+ " Absolute Encoder Offset",
+ encoder.getDeviceID()
+ " Absolute Encoder Offset",
Alert.AlertType.WARNING);

/**
Expand Down Expand Up @@ -111,16 +119,13 @@ public double getAbsolutePosition()
readingError = false;
MagnetHealthValue strength = encoder.getMagnetHealth().getValue();

if (strength != MagnetHealthValue.Magnet_Green)
{
magnetFieldLessThanIdeal.set(true);
} else magnetFieldLessThanIdeal.set(false);
magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green);
if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
{
readingError = true;
readingFaulty.set(true);
return 0;
}else
} else
{
readingFaulty.set(false);
}
Expand All @@ -141,7 +146,10 @@ public double getAbsolutePosition()
{
readingError = true;
readingIgnored.set(true);
} else readingIgnored.set(false);
} else
{
readingIgnored.set(false);
}

return angle.getValue() * 360;
}
Expand Down Expand Up @@ -176,10 +184,11 @@ public boolean setAbsoluteEncoderOffset(double offset)
error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
cannotSetOffset.setText(
"Failure to set CANCoder "
+ encoder.getDeviceID()
+ " Absolute Encoder Offset Error: "
+ error);
if (error == StatusCode.OK) {
+ encoder.getDeviceID()
+ " Absolute Encoder Offset Error: "
+ error);
if (error == StatusCode.OK)
{
cannotSetOffset.set(false);
return true;
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/swervelib/encoders/CanAndCoderSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public CanAndCoderSwerve(int canid)

/**
* Reset the encoder to factory defaults.
*
* <p>
* This will not clear the stored zero offset.
*/
@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,10 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
* Inversion state.
*/
private boolean isInverted;
/** An {@link Alert} for if the encoder cannot report accurate velocities. */
private Alert inaccurateVelocities = new Alert(
/**
* An {@link Alert} for if the encoder cannot report accurate velocities.
*/
private Alert inaccurateVelocities = new Alert(
"Encoders",
"The PWM Duty Cycle encoder may not report accurate velocities!",
Alert.AlertType.WARNING_TRACE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.revrobotics.REVLibError;
import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkAnalogSensor.Mode;

import java.util.function.Supplier;
import swervelib.motors.SwerveMotor;
import swervelib.telemetry.Alert;
Expand All @@ -18,14 +17,18 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
/**
* The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
*/
public SparkAnalogSensor encoder;
/** An {@link Alert} for if there is a failure configuring the encoder. */
private Alert failureConfiguring = new Alert(
public SparkAnalogSensor encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
private Alert failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
Alert.AlertType.WARNING_TRACE);
/** An {@link Alert} for if the absolute encoder does not support integrated offsets. */
private Alert doesNotSupportIntegratedOffsets = new Alert(
/**
* An {@link Alert} for if the absolute encoder does not support integrated offsets.
*/
private Alert doesNotSupportIntegratedOffsets = new Alert(
"Encoders",
"SparkMax Analog Sensors do not support integrated offsets",
Alert.AlertType.WARNING_TRACE);
Expand Down
11 changes: 6 additions & 5 deletions src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.SparkAbsoluteEncoder.Type;

import java.util.function.Supplier;
import swervelib.motors.SwerveMotor;
import swervelib.telemetry.Alert;
Expand All @@ -18,13 +17,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
*/
public AbsoluteEncoder encoder;
/** An {@link Alert} for if there is a failure configuring the encoder. */
private Alert failureConfiguring = new Alert(
public AbsoluteEncoder encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
private Alert failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
Alert.AlertType.WARNING_TRACE);
private Alert offsetFailure = new Alert(
private Alert offsetFailure = new Alert(
"Encoders",
"Failure to set Absolute Encoder Offset",
Alert.AlertType.WARNING_TRACE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ public abstract class SwerveAbsoluteEncoder

/**
* Get the velocity in degrees/sec.
*
* @return velocity in degrees/sec.
*/
public abstract double getVelocity();
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/swervelib/imu/ADIS16470Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;

Expand Down
9 changes: 5 additions & 4 deletions src/main/java/swervelib/imu/NavXSwerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,8 @@
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.telemetry.Alert;

import java.util.Optional;
import swervelib.telemetry.Alert;

/**
* Communicates with the NavX as the IMU.
Expand All @@ -24,8 +23,10 @@ public class NavXSwerve extends SwerveIMU
/**
* Offset for the NavX.
*/
private Rotation3d offset = new Rotation3d();
/** An {@link Alert} for if there is an error instantiating the NavX. */
private Rotation3d offset = new Rotation3d();
/**
* An {@link Alert} for if there is an error instantiating the NavX.
*/
private Alert navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE);

/**
Expand Down
17 changes: 11 additions & 6 deletions src/main/java/swervelib/imu/Pigeon2Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -88,11 +88,14 @@ public void setOffset(Rotation3d offset)
public Rotation3d getRawRotation3d()
{
// TODO: Switch to suppliers.
StatusSignal<Double> w = imu.getQuatW();
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> z = imu.getQuatZ();
return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue()));
StatusSignal<Double> w = imu.getQuatW();
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> z = imu.getQuatZ();
return new Rotation3d(new Quaternion(w.refresh().getValue(),
x.refresh().getValue(),
y.refresh().getValue(),
z.refresh().getValue()));
}

/**
Expand Down Expand Up @@ -120,7 +123,9 @@ public Optional<Translation3d> getAccel()
StatusSignal<Double> yAcc = imu.getAccelerationX();
StatusSignal<Double> zAcc = imu.getAccelerationX();

return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0));
return Optional.of(new Translation3d(xAcc.refresh().getValue(),
yAcc.refresh().getValue(),
zAcc.refresh().getValue()).times(9.81 / 16384.0));
}

/**
Expand Down
Loading

0 comments on commit a63d238

Please sign in to comment.