Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions photon-lib/py/disableUsingDevBuilds.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
python3 -m pip config unset global.find-links
1 change: 1 addition & 0 deletions photon-lib/py/enableUsingDevBuilds.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
python3 -m pip config set global.find-links dist
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ public static class Vision {

// The layout of the AprilTags on the field
public static final AprilTagFieldLayout kTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ public void setDesiredState(
SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) {
Rotation2d currentRotation = getAbsoluteHeading();
// Avoid turning more than 90 degrees by inverting speed on large angle changes
desiredState = SwerveModuleState.optimize(desiredState, currentRotation);
desiredState.optimize(currentRotation);

this.desiredState = desiredState;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ public static class Vision {

// The layout of the AprilTags on the field
public static final AprilTagFieldLayout kTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ public void setDesiredState(
SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) {
Rotation2d currentRotation = getAbsoluteHeading();
// Avoid turning more than 90 degrees by inverting speed on large angle changes
desiredState = SwerveModuleState.optimize(desiredState, currentRotation);
desiredState.optimize(currentRotation);

this.desiredState = desiredState;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ public static class Vision {

// The layout of the AprilTags on the field
public static final AprilTagFieldLayout kTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

// The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ public void setDesiredState(
SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) {
Rotation2d currentRotation = getAbsoluteHeading();
// Avoid turning more than 90 degrees by inverting speed on large angle changes
desiredState = SwerveModuleState.optimize(desiredState, currentRotation);
desiredState.optimize(currentRotation);

this.desiredState = desiredState;
}
Expand Down
12 changes: 5 additions & 7 deletions photonlib-python-examples/aimandrange/swervemodule.py
Original file line number Diff line number Diff line change
Expand Up @@ -142,25 +142,23 @@ def setDesiredState(
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(
Expand Down
12 changes: 5 additions & 7 deletions photonlib-python-examples/aimattarget/swervemodule.py
Original file line number Diff line number Diff line change
Expand Up @@ -142,25 +142,23 @@ def setDesiredState(
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(
Expand Down
12 changes: 5 additions & 7 deletions photonlib-python-examples/poseest/swervemodule.py
Original file line number Diff line number Diff line change
Expand Up @@ -141,25 +141,23 @@ def setDesiredState(
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
)
self.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()
self.desiredState.speed *= (self.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(), self.desiredState.speed
)

driveFeedforward = self.driveFeedforward.calculate(state.speed)
driveFeedforward = self.driveFeedforward.calculate(self.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(), self.desiredState.angle.radians()
)

self.driveMotor.setVoltage(driveOutput + driveFeedforward)
Expand Down
25 changes: 25 additions & 0 deletions photonlib-python-examples/run.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
# Check if the first argument is provided
if [ $# -eq 0 ]
then
echo "Error: No example-to-run provided."
exit 1
fi

# To run any example, we want to use photonlib out of this repo
# Build the wheel first
pushd ../photon-lib/py
if [ -d build ]
then rm -rdf build
fi
python3 setup.py bdist_wheel
popd

# Add the output directory to PYTHONPATH to make sure it gets picked up
export PHOTONLIBPY_ROOT=../photon-lib/py
export PYTHONPATH=$PHOTONLIBPY_ROOT

# Move to the right example folder
cd $1

# Run the example
robotpy sim