Skip to content

Commit

Permalink
Only support fully-mutable wheel positions objects
Browse files Browse the repository at this point in the history
We never had any like this, and C++ required full mutability anyways
  • Loading branch information
KangarooKoala committed Jul 13, 2024
1 parent 2cc469b commit 7ed1d1d
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 53 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -121,11 +121,10 @@ public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions posi
}

@Override
public DifferentialDriveWheelPositions copyInto(
DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions buffer) {
buffer.leftMeters = positions.leftMeters;
buffer.rightMeters = positions.rightMeters;
return buffer;
public void copyInto(
DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) {
output.leftMeters = positions.leftMeters;
output.rightMeters = positions.rightMeters;
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,43 +49,17 @@ public interface Kinematics<S, P> extends Interpolator<P> {
/**
* Returns a copy of the wheel positions object.
*
* <p>Implementation note:
*
* <ul>
* <li>If {@code P} is (deeply) immutable, this should return {@code positions}.
* <li>Otherwise, a new copy should be created and returned.
* </ul>
*
* @param positions The wheel positions object to copy.
* @return A copy.
*/
P copy(P positions);

/**
* Returns a copy of the wheel positions object, possibly reusing storage in the buffer.
*
* <p>This is meant to be used like this:
*
* <pre>
* m_buffer = m_kinematics.copyInto(positions, m_buffer);
* </pre>
*
* <p>Implementation note:
*
* <ul>
* <li>If {@code P} is (deeply) immutable, then like {@link #copy}, this should return {@code
* positions}.
* <li>If {@code P} is completely mutable, then this should copy into {@code buffer} and return
* it.
* <li>Otherwise (some members are immutable and some are mutable), a new copy should be created
* and returned.
* </ul>
* Copies the value of the wheel positions object into the output.
*
* @param positions The wheel positions object to copy. Must not be modified.
* @param buffer A buffer available for overwriting. The state after the call is undefined.
* @return The copy. Identity is undefined.
* @param positions The wheel positions object to copy. Will not be modified.
* @param output The output variable of the copy operation. Will have the same value as the
* positions object after the call.
*/
default P copyInto(P positions, P buffer) {
return copy(positions);
}
void copyInto(P positions, P output);
}
Original file line number Diff line number Diff line change
Expand Up @@ -267,13 +267,11 @@ public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) {
}

@Override
public MecanumDriveWheelPositions copyInto(
MecanumDriveWheelPositions positions, MecanumDriveWheelPositions buffer) {
buffer.frontLeftMeters = positions.frontLeftMeters;
buffer.frontRightMeters = positions.frontRightMeters;
buffer.rearLeftMeters = positions.rearLeftMeters;
buffer.rearRightMeters = positions.rearRightMeters;
return buffer;
public void copyInto(MecanumDriveWheelPositions positions, MecanumDriveWheelPositions output) {
output.frontLeftMeters = positions.frontLeftMeters;
output.frontRightMeters = positions.frontRightMeters;
output.rearLeftMeters = positions.rearLeftMeters;
output.rearRightMeters = positions.rearRightMeters;
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ public class Odometry<T> {

private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private T m_previousWheelPositions;
private final T m_previousWheelPositions;

/**
* Constructs an Odometry object.
Expand Down Expand Up @@ -60,7 +60,7 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousWheelPositions = m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
}

/**
Expand Down Expand Up @@ -90,7 +90,7 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {

var newPose = m_poseMeters.exp(twist);

m_previousWheelPositions = m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -393,16 +393,14 @@ public SwerveModulePosition[] copy(SwerveModulePosition[] positions) {
}

@Override
public SwerveModulePosition[] copyInto(
SwerveModulePosition[] positions, SwerveModulePosition[] buffer) {
if (positions.length != buffer.length) {
return copy(positions);
public void copyInto(SwerveModulePosition[] positions, SwerveModulePosition[] output) {
if (positions.length != output.length) {
throw new IllegalArgumentException("Inconsistent number of modules!");
}
for (int i = 0; i < positions.length; ++i) {
buffer[i].distanceMeters = positions[i].distanceMeters;
buffer[i].angle = positions[i].angle;
output[i].distanceMeters = positions[i].distanceMeters;
output[i].angle = positions[i].angle;
}
return buffer;
}

@Override
Expand Down

0 comments on commit 7ed1d1d

Please sign in to comment.