Initial commit

photonlib-python-examples/aimattarget/.gitignore

@@ -0,0 +1,175 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/
+
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+*-window.json
+networktables.json
+
+__pycache__

photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json

@@ -0,0 +1 @@
+{"teamNumber": 1736}

photonlib-python-examples/aimattarget/drivetrain.py

@@ -0,0 +1,203 @@
+###################################################################################
+# MIT License
+#
+# Copyright (c) PhotonVision
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+###################################################################################
+
+
+import math
+
+import swervemodule
+import wpilib
+import wpilib.simulation
+import wpimath.geometry
+import wpimath.kinematics
+
+kMaxSpeed = 3.0  # 3 meters per second
+kMaxAngularSpeed = math.pi  # 1/2 rotation per second
+
+
+class Drivetrain:
+    """
+    Represents a swerve drive style drivetrain.
+    """
+
+    def __init__(self) -> None:
+        self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381)
+        self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381)
+        self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
+        self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
+
+        self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
+        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
+        self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11, 3)
+        self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15, 4)
+
+        self.debugField = wpilib.Field2d()
+        wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField)
+
+        self.gyro = wpilib.AnalogGyro(0)
+        self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro)
+
+        self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics(
+            self.frontLeftLocation,
+            self.frontRightLocation,
+            self.backLeftLocation,
+            self.backRightLocation,
+        )
+
+        self.odometry = wpimath.kinematics.SwerveDrive4Odometry(
+            self.kinematics,
+            self.gyro.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+        )
+
+        self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
+
+        self.gyro.reset()
+
+    def drive(
+        self,
+        xSpeed: float,
+        ySpeed: float,
+        rot: float,
+        fieldRelative: bool,
+        periodSeconds: float,
+    ) -> None:
+        """
+        Method to drive the robot using joystick info.
+        :param xSpeed: Speed of the robot in the x direction (forward).
+        :param ySpeed: Speed of the robot in the y direction (sideways).
+        :param rot: Angular rate of the robot.
+        :param fieldRelative: Whether the provided x and y speeds are relative to the field.
+        :param periodSeconds: Time
+        """
+        swerveModuleStates = self.kinematics.toSwerveModuleStates(
+            wpimath.kinematics.ChassisSpeeds.discretize(
+                (
+                    wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds(
+                        xSpeed, ySpeed, rot, self.gyro.getRotation2d()
+                    )
+                    if fieldRelative
+                    else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot)
+                ),
+                periodSeconds,
+            )
+        )
+        wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds(
+            swerveModuleStates, kMaxSpeed
+        )
+        self.frontLeft.setDesiredState(swerveModuleStates[0])
+        self.frontRight.setDesiredState(swerveModuleStates[1])
+        self.backLeft.setDesiredState(swerveModuleStates[2])
+        self.backRight.setDesiredState(swerveModuleStates[3])
+
+        self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
+
+    def updateOdometry(self) -> None:
+        """Updates the field relative position of the robot."""
+        self.odometry.update(
+            self.gyro.getRotation2d(),
+            (
+                self.frontLeft.getPosition(),
+                self.frontRight.getPosition(),
+                self.backLeft.getPosition(),
+                self.backRight.getPosition(),
+            ),
+        )
+
+    def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
+        return [
+            self.frontLeft.getState(),
+            self.frontRight.getState(),
+            self.backLeft.getState(),
+            self.backRight.getState(),
+        ]
+
+    def getModulePoses(self) -> list[wpimath.geometry.Pose2d]:
+        p = self.odometry.getPose()
+        flTrans = wpimath.geometry.Transform2d(
+            self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()
+        )
+        frTrans = wpimath.geometry.Transform2d(
+            self.frontRightLocation, self.frontRight.getAbsoluteHeading()
+        )
+        blTrans = wpimath.geometry.Transform2d(
+            self.backLeftLocation, self.backLeft.getAbsoluteHeading()
+        )
+        brTrans = wpimath.geometry.Transform2d(
+            self.backRightLocation, self.backRight.getAbsoluteHeading()
+        )
+        return [
+            p.transformBy(flTrans),
+            p.transformBy(frTrans),
+            p.transformBy(blTrans),
+            p.transformBy(brTrans),
+        ]
+
+    def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
+        return self.kinematics.toChassisSpeeds(self.getModuleStates())
+
+    def log(self):
+        table = "Drive/"
+
+        pose = self.odometry.getPose()
+        wpilib.SmartDashboard.putNumber(table + "X", pose.X())
+        wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
+        wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
+
+        chassisSpeeds = self.getChassisSpeeds()
+        wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
+        wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
+        wpilib.SmartDashboard.putNumber(
+            table + "Omega Degrees", chassisSpeeds.omega_dps
+        )
+
+        wpilib.SmartDashboard.putNumber(
+            table + "Target VX", self.targetChassisSpeeds.vx
+        )
+        wpilib.SmartDashboard.putNumber(
+            table + "Target VY", self.targetChassisSpeeds.vy
+        )
+        wpilib.SmartDashboard.putNumber(
+            table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps
+        )
+
+        self.frontLeft.log()
+        self.frontRight.log()
+        self.backLeft.log()
+        self.backRight.log()
+
+        self.debugField.getRobotObject().setPose(self.odometry.getPose())
+        self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses())
+
+    def simulationPeriodic(self):
+        self.frontLeft.simulationPeriodic()
+        self.frontRight.simulationPeriodic()
+        self.backLeft.simulationPeriodic()
+        self.backRight.simulationPeriodic()
+        self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps)
+        self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)

photonlib-python-examples/aimattarget/pyproject.toml

@@ -0,0 +1,33 @@
+#
+# Use this configuration file to control what RobotPy packages are installed
+# on your RoboRIO
+#
+
+[tool.robotpy]
+
+# Version of robotpy this project depends on
+robotpy_version = "2024.3.2.2"
+
+# Which extra RobotPy components should be installed
+# -> equivalent to `pip install robotpy[extra1, ...]
+robotpy_extras = [
+    # "all",
+    # "apriltag",
+    # "commands2",
+    # "cscore",
+    # "navx",
+    # "pathplannerlib",
+    # "phoenix5",
+    # "phoenix6",
+    # "photonvision",
+    # "playingwithfusion",
+    # "rev",
+    # "romi",
+    # "sim",
+    # "xrp",
+]
+
+# Other pip packages to install
+requires = [
+    "photonlibpy"
+]

photonlib-python-examples/aimattarget/robot.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+###################################################################################
+# MIT License
+#
+# Copyright (c) PhotonVision
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+###################################################################################
+
+
+import drivetrain
+import wpilib
+from photonlibpy import PhotonCamera
+
+VISION_TURN_kP = 0.01
+
+
+class MyRobot(wpilib.TimedRobot):
+    def robotInit(self) -> None:
+        """Robot initialization function"""
+        self.controller = wpilib.XboxController(0)
+        self.swerve = drivetrain.Drivetrain()
+        self.cam = PhotonCamera("YOUR CAMERA NAME")
+
+    def robotPeriodic(self) -> None:
+        self.swerve.updateOdometry()
+        self.swerve.log()
+
+    def teleopPeriodic(self) -> None:
+        xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed
+        ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed
+        rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed
+
+        # Get information from the camera
+        targetYaw = 0.0
+        targetVisible = False
+        results = self.cam.getAllUnreadResults()
+        if len(results) > 0:
+            result = results[-1]  # take the most recent result the camera had
+            for target in result.getTargets():
+                if target.getFiducialId() == 7:
+                    # Found tag 7, record its information
+                    targetVisible = True
+                    targetYaw = target.getYaw()
+
+        if self.controller.getAButton() and targetVisible:
+            # Driver wants auto-alignment to tag 7
+            # And, tag 7 is in sight, so we can turn toward it.
+            # Override the driver's turn command with an automatic one that turns toward the tag.
+            rot = -1.0 * targetYaw * VISION_TURN_kP * drivetrain.kMaxAngularSpeed
+
+        self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod())
+
+    def _simulationPeriodic(self) -> None:
+        self.swerve.simulationPeriodic()
+        return super()._simulationPeriodic()

photonlib-python-examples/aimattarget/simgui-ds.json

@@ -0,0 +1,100 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decayRate": 0.0,
+          "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
+        }
+      ],
+      "axisCount": 6,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

photonlib-python-examples/aimattarget/simgui.json

@@ -0,0 +1,63 @@
+{
+  "HALProvider": {
+    "Encoders": {
+      "0": {
+        "header": {
+          "open": true
+        }
+      },
+      "window": {
+        "visible": true
+      }
+    },
+    "Other Devices": {
+      "AnalogGyro[0]": {
+        "header": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Drivetrain Debug": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Drivetrain Debug": {
+        "SwerveModules": {
+          "image": "swerve_module.png",
+          "length": 0.5,
+          "width": 0.5
+        },
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "Drivetrain Debug": {
+          "double[]##v_/SmartDashboard/Drivetrain Debug/Robot": {
+            "open": true
+          }
+        },
+        "Module 1": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

photonlib-python-examples/aimattarget/swervemodule.py

@@ -0,0 +1,219 @@
+###################################################################################
+# MIT License
+#
+# Copyright (c) PhotonVision
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+###################################################################################
+
+import math
+
+import wpilib
+import wpilib.simulation
+import wpimath.controller
+import wpimath.filter
+import wpimath.geometry
+import wpimath.kinematics
+import wpimath.trajectory
+import wpimath.units
+
+kWheelRadius = 0.0508
+kEncoderResolution = 4096
+kModuleMaxAngularVelocity = math.pi
+kModuleMaxAngularAcceleration = math.tau
+
+
+class SwerveModule:
+    def __init__(
+        self,
+        driveMotorChannel: int,
+        turningMotorChannel: int,
+        driveEncoderChannelA: int,
+        driveEncoderChannelB: int,
+        turningEncoderChannelA: int,
+        turningEncoderChannelB: int,
+        moduleNumber: int,
+    ) -> None:
+        """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
+
+        :param driveMotorChannel:      PWM output for the drive motor.
+        :param turningMotorChannel:    PWM output for the turning motor.
+        :param driveEncoderChannelA:   DIO input for the drive encoder channel A
+        :param driveEncoderChannelB:   DIO input for the drive encoder channel B
+        :param turningEncoderChannelA: DIO input for the turning encoder channel A
+        :param turningEncoderChannelB: DIO input for the turning encoder channel B
+        """
+        self.moduleNumber = moduleNumber
+        self.desiredState = wpimath.kinematics.SwerveModuleState()
+
+        self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel)
+        self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel)
+
+        self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB)
+        self.turningEncoder = wpilib.Encoder(
+            turningEncoderChannelA, turningEncoderChannelB
+        )
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.turningPIDController = wpimath.controller.PIDController(30, 0, 0)
+
+        # Gains are for example purposes only - must be determined for your own robot!
+        self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
+        self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)
+
+        # Set the distance per pulse for the drive encoder. We can simply use the
+        # distance traveled for one rotation of the wheel divided by the encoder
+        # resolution.
+        self.driveEncoder.setDistancePerPulse(
+            math.tau * kWheelRadius / kEncoderResolution
+        )
+
+        # Set the distance (in this case, angle) in radians per pulse for the turning encoder.
+        # This is the the angle through an entire rotation (2 * pi) divided by the
+        # encoder resolution.
+        self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution)
+
+        # Limit the PID Controller's input range between -pi and pi and set the input
+        # to be continuous.
+        self.turningPIDController.enableContinuousInput(-math.pi, math.pi)
+
+        # Simulation Support
+        self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder)
+        self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder)
+        self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor)
+        self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor)
+        self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
+            0.1, 0.02
+        )
+        self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(
+            0.0001, 0.02
+        )
+        self.simTurningEncoderPos = 0
+        self.simDrivingEncoderPos = 0
+
+    def getState(self) -> wpimath.kinematics.SwerveModuleState:
+        """Returns the current state of the module.
+
+        :returns: The current state of the module.
+        """
+        return wpimath.kinematics.SwerveModuleState(
+            self.driveEncoder.getRate(),
+            wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def getPosition(self) -> wpimath.kinematics.SwerveModulePosition:
+        """Returns the current position of the module.
+
+        :returns: The current position of the module.
+        """
+        return wpimath.kinematics.SwerveModulePosition(
+            self.driveEncoder.getDistance(),
+            wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()),
+        )
+
+    def setDesiredState(
+        self, desiredState: wpimath.kinematics.SwerveModuleState
+    ) -> None:
+        """Sets the desired state for the module.
+
+        :param desiredState: Desired state with speed and angle.
+        """
+        self.desiredState = desiredState
+
+        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
+        )
+
+        # 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()
+
+        # Calculate the drive output from the drive PID controller.
+        driveOutput = self.drivePIDController.calculate(
+            self.driveEncoder.getRate(), state.speed
+        )
+
+        driveFeedforward = self.driveFeedforward.calculate(state.speed)
+
+        # Calculate the turning motor output from the turning PID controller.
+        turnOutput = self.turningPIDController.calculate(
+            self.turningEncoder.getDistance(), state.angle.radians()
+        )
+
+        turnFeedforward = self.turnFeedforward.calculate(
+            self.turningPIDController.getSetpoint()
+        )
+
+        self.driveMotor.setVoltage(driveOutput + driveFeedforward)
+        self.turningMotor.setVoltage(turnOutput + turnFeedforward)
+
+    def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d:
+        return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
+
+    def log(self) -> None:
+        state = self.getState()
+
+        table = "Module " + str(self.moduleNumber) + "/"
+        wpilib.SmartDashboard.putNumber(
+            table + "Steer Degrees",
+            math.degrees(wpimath.angleModulus(state.angle.radians())),
+        )
+        wpilib.SmartDashboard.putNumber(
+            table + "Steer Target Degrees",
+            math.degrees(self.turningPIDController.getSetpoint()),
+        )
+        wpilib.SmartDashboard.putNumber(table + "Drive Velocity Feet", state.speed_fps)
+        wpilib.SmartDashboard.putNumber(
+            table + "Drive Velocity Target Feet", self.desiredState.speed_fps
+        )
+        wpilib.SmartDashboard.putNumber(
+            table + "Drive Voltage", self.driveMotor.get() * 12.0
+        )
+        wpilib.SmartDashboard.putNumber(
+            table + "Steer Voltage", self.turningMotor.get() * 12.0
+        )
+
+    def simulationPeriodic(self) -> None:
+        driveVoltage = (
+            self.simDrivingMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage()
+        )
+        turnVoltage = (
+            self.simTurningMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage()
+        )
+        driveSpdRaw = (
+            driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0, 0)
+        )
+        turnSpdRaw = (
+            turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0, 0)
+        )
+        driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
+        turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
+        self.simDrivingEncoderPos += 0.02 * driveSpd
+        self.simTurningEncoderPos += 0.02 * turnSpd
+        self.simDriveEncoder.setDistance(self.simDrivingEncoderPos)
+        self.simDriveEncoder.setRate(driveSpd)
+        self.simTurningEncoder.setDistance(self.simTurningEncoderPos)
+        self.simTurningEncoder.setRate(turnSpd)

photonlib-python-examples/aimattarget/tests/pyfrc_test.py

@@ -0,0 +1,4 @@
+"""
+    This test module imports tests that come with pyfrc, and can be used
+    to test basic functionality of just about any robot.
+"""