diff --git a/photon-lib/py/disableUsingDevBuilds.sh b/photon-lib/py/disableUsingDevBuilds.sh new file mode 100755 index 0000000000..26646fd2fa --- /dev/null +++ b/photon-lib/py/disableUsingDevBuilds.sh @@ -0,0 +1 @@ +python3 -m pip config unset global.find-links diff --git a/photon-lib/py/enableUsingDevBuilds.sh b/photon-lib/py/enableUsingDevBuilds.sh new file mode 100755 index 0000000000..db0f30d6c8 --- /dev/null +++ b/photon-lib/py/enableUsingDevBuilds.sh @@ -0,0 +1 @@ +python3 -m pip config set global.find-links dist diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java index 921f57a875..da4d3d7fa0 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java @@ -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.) diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 4615d29217..5d77dcce51 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -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; } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java index 921f57a875..da4d3d7fa0 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java @@ -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.) diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 4615d29217..5d77dcce51 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -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; } diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java index 1cea631e93..837fd8f81c 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java @@ -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.) diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java index 4615d29217..5d77dcce51 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -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; } diff --git a/photonlib-python-examples/aimandrange/swervemodule.py b/photonlib-python-examples/aimandrange/swervemodule.py index 6cbba18d5d..d269c620c3 100644 --- a/photonlib-python-examples/aimandrange/swervemodule.py +++ b/photonlib-python-examples/aimandrange/swervemodule.py @@ -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( diff --git a/photonlib-python-examples/aimattarget/swervemodule.py b/photonlib-python-examples/aimattarget/swervemodule.py index 6cbba18d5d..d269c620c3 100644 --- a/photonlib-python-examples/aimattarget/swervemodule.py +++ b/photonlib-python-examples/aimattarget/swervemodule.py @@ -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( diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py index b38e26aed9..59cf9e9c3b 100644 --- a/photonlib-python-examples/poseest/swervemodule.py +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -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) diff --git a/photonlib-python-examples/run.sh b/photonlib-python-examples/run.sh new file mode 100755 index 0000000000..c5a82709d3 --- /dev/null +++ b/photonlib-python-examples/run.sh @@ -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