Fix deprecation warnings in PhotonLib examples (#1699)

The following deprecation warnings have been fixed:
- `SwerveModuleState.optimize(desiredState, currentRotation);`, which is
now an instance method
- `AprilTagFields.kDefaultField.loadAprilTagLayoutField();`, which is
now `AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);`

WIP:
- [x] C++
- [x] Python
This commit is contained in:
DeltaDizzy
2025-01-11 23:30:24 -05:00
committed by GitHub
parent 159b848234
commit d487e1c466
12 changed files with 48 additions and 27 deletions

View File

@@ -142,25 +142,23 @@ class SwerveModule:
encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
# Optimize the reference state to avoid spinning further than 90 degrees
state = wpimath.kinematics.SwerveModuleState.optimize(
self.desiredState, encoderRotation
)
desiredState.optimize(encoderRotation)
# Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
# direction of travel that can occur when modules change directions. This results in smoother
# driving.
state.speed *= (state.angle - encoderRotation).cos()
desiredState.speed *= (desiredState.angle - encoderRotation).cos()
# Calculate the drive output from the drive PID controller.
driveOutput = self.drivePIDController.calculate(
self.driveEncoder.getRate(), state.speed
self.driveEncoder.getRate(), desiredState.speed
)
driveFeedforward = self.driveFeedforward.calculate(state.speed)
driveFeedforward = self.driveFeedforward.calculate(desiredState.speed)
# Calculate the turning motor output from the turning PID controller.
turnOutput = self.turningPIDController.calculate(
self.turningEncoder.getDistance(), state.angle.radians()
self.turningEncoder.getDistance(), desiredState.angle.radians()
)
turnFeedforward = self.turnFeedforward.calculate(