Python examples fixed up and added

photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java

@@ -44,7 +44,7 @@ public class Robot extends TimedRobot {
     private PhotonCamera camera;
 
     private final double VISION_TURN_kP = 0.01;
-    private final double VISION_DES_ANGLE_rad = 0.0;
+    private final double VISION_DES_ANGLE_deg = 0.0;
     private final double VISION_STRAFE_kP = 0.5;
     private final double VISION_DES_RANGE_m = 1.25;
 
@@ -121,7 +121,7 @@ public void teleopPeriodic() {
             // Override the driver's turn and fwd/rev command with an automatic one
             // That turns toward the tag, and gets the range right.
             turn =
-                    (VISION_DES_ANGLE_rad - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
+                    (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed;
             forward =
                     (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed;
         }

photonlib-python-examples/aimandrange/.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/aimandrange/.wpilib/wpilib_preferences.json

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

photonlib-python-examples/aimandrange/drivetrain.py

@@ -0,0 +1,168 @@
+#
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+#
+
+import math
+import wpilib
+import wpilib.simulation
+import wpimath.geometry
+import wpimath.kinematics
+import swervemodule
+
+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)