diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py
index 13be91193..6f6fea2ce 100644
--- a/photon-lib/py/photonlibpy/__init__.py
+++ b/photon-lib/py/photonlibpy/__init__.py
@@ -1,4 +1,19 @@
-# No one here but us chickens
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
from .packet import Packet # noqa
from .estimatedRobotPose import EstimatedRobotPose # noqa
diff --git a/photon-lib/py/photonlibpy/estimatedRobotPose.py b/photon-lib/py/photonlibpy/estimatedRobotPose.py
index c789e3996..491f71c83 100644
--- a/photon-lib/py/photonlibpy/estimatedRobotPose.py
+++ b/photon-lib/py/photonlibpy/estimatedRobotPose.py
@@ -1,3 +1,20 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
+
from dataclasses import dataclass
from typing import TYPE_CHECKING
diff --git a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
index 3fab14335..a40d07fe4 100644
--- a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
+++ b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
@@ -24,7 +24,6 @@
class MultiTargetPNPResultSerde:
-
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "ffc1cb847deb6e796a583a5b1885496b"
MESSAGE_FORMAT = "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"
diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py
index 531e9ec89..b5ce2d8a9 100644
--- a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py
+++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py
@@ -24,7 +24,6 @@
class PhotonPipelineMetadataSerde:
-
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2"
MESSAGE_FORMAT = (
diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
index 2823e74c0..f638b3bb1 100644
--- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
+++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
@@ -24,7 +24,6 @@
class PhotonPipelineResultSerde:
-
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "cb3e1605048ba49325888eb797399fe2"
MESSAGE_FORMAT = "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] targets;MultiTargetPNPResult? multiTagResult;"
diff --git a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py b/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py
index 64468f87c..c728295c6 100644
--- a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py
+++ b/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py
@@ -24,7 +24,6 @@
class PhotonTrackedTargetSerde:
-
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "8fdada56b9162f2e32bd24f0055d7b60"
MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] detectedCorners;"
diff --git a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py b/photon-lib/py/photonlibpy/generated/PnpResultSerde.py
index b96789e2d..aaeb74c86 100644
--- a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py
+++ b/photon-lib/py/photonlibpy/generated/PnpResultSerde.py
@@ -24,7 +24,6 @@
class PnpResultSerde:
-
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "0d1f2546b00f24718e30f38d206d4491"
MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
diff --git a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py b/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py
index dedf43778..beccc9e2d 100644
--- a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py
+++ b/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py
@@ -24,7 +24,6 @@
class TargetCornerSerde:
-
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "22b1ff7551d10215af6fb3672fe4eda8"
MESSAGE_FORMAT = "float64 x;float64 y;"
diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py
index a05c20c04..53c3f84c5 100644
--- a/photon-lib/py/photonlibpy/packet.py
+++ b/photon-lib/py/photonlibpy/packet.py
@@ -1,3 +1,20 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
+
import struct
from typing import Any, Optional, Type
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py
index a3e21b2d9..32f337b20 100644
--- a/photon-lib/py/photonlibpy/photonCamera.py
+++ b/photon-lib/py/photonlibpy/photonCamera.py
@@ -1,3 +1,20 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
+
from enum import Enum
from typing import List
import ntcore
diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py
index eaa07a5d3..419c64102 100644
--- a/photon-lib/py/photonlibpy/photonPoseEstimator.py
+++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py
@@ -1,3 +1,20 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
+
import enum
from typing import Optional
diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py
index c81a64dbc..e761f5bb9 100644
--- a/photon-lib/py/test/photonPoseEstimator_test.py
+++ b/photon-lib/py/test/photonPoseEstimator_test.py
@@ -1,3 +1,20 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
+
# from photonlibpy import MultiTargetPNPResult, PnpResult
# from photonlibpy import PhotonPipelineResult
# from photonlibpy import PhotonPoseEstimator, PoseStrategy
diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py
index a1673f3a5..dddce605f 100644
--- a/photon-lib/py/test/photonlibpy_test.py
+++ b/photon-lib/py/test/photonlibpy_test.py
@@ -1,3 +1,20 @@
+###############################################################################
+## Copyright (C) Photon Vision.
+###############################################################################
+## This program is free software: you can redistribute it and/or modify
+## it under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## This program is distributed in the hope that it will be useful,
+## but WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with this program. If not, see .
+###############################################################################
+
from time import sleep
from photonlibpy import PhotonCamera
import ntcore
@@ -5,7 +22,6 @@
def test_roundTrip():
-
ntcore.NetworkTableInstance.getDefault().stopServer()
ntcore.NetworkTableInstance.getDefault().setServer("localhost")
ntcore.NetworkTableInstance.getDefault().startClient4("meme")
diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h
index e03de9c6a..df2c05d92 100644
--- a/photon-lib/src/main/native/include/photon/PhotonCamera.h
+++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h
@@ -87,8 +87,8 @@ class PhotonCamera {
*/
std::vector GetAllUnreadResults();
- [[deprecated("Replace with GetAllUnreadResults")]]
- PhotonPipelineResult GetLatestResult();
+ [[deprecated("Replace with GetAllUnreadResults")]] PhotonPipelineResult
+ GetLatestResult();
/**
* Toggles driver mode.
diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
index ebb6a9fff..09bb4a08b 100644
--- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
+++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
@@ -53,8 +53,8 @@ static std::vector GetConvexHull(
return convexPoints;
}
-[[maybe_unused]]
-static cv::RotatedRect GetMinAreaRect(const std::vector& points) {
+[[maybe_unused]] static cv::RotatedRect GetMinAreaRect(
+ const std::vector& points) {
return cv::minAreaRect(points);
}
@@ -144,8 +144,7 @@ static std::vector RotationToRVec(
return cv::boundingRect(points);
}
-[[maybe_unused]]
-static std::vector ProjectPoints(
+[[maybe_unused]] static std::vector ProjectPoints(
const Eigen::Matrix& cameraMatrix,
const Eigen::Matrix& distCoeffs,
const RotTrlTransform3d& camRt,
diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
index 8924e5d0c..b2dace10a 100644
--- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
+++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
@@ -32,8 +32,7 @@
namespace photon {
namespace VisionEstimation {
-[[maybe_unused]]
-static std::vector GetVisibleLayoutTags(
+[[maybe_unused]] static std::vector GetVisibleLayoutTags(
const std::vector& visTags,
const frc::AprilTagFieldLayout& layout) {
std::vector retVal{};
diff --git a/photonlib-python-examples/aimandrange/.gitignore b/photonlib-python-examples/aimandrange/.gitignore
index fa0d62106..244ad813e 100644
--- a/photonlib-python-examples/aimandrange/.gitignore
+++ b/photonlib-python-examples/aimandrange/.gitignore
@@ -172,4 +172,4 @@ out/
*-window.json
networktables.json
-__pycache__
\ No newline at end of file
+__pycache__
diff --git a/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json
index 2b42e34e6..f54f38af6 100644
--- a/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json
+++ b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json
@@ -1 +1 @@
-{"teamNumber": 1736}
\ No newline at end of file
+{"teamNumber": 1736}
diff --git a/photonlib-python-examples/aimandrange/drivetrain.py b/photonlib-python-examples/aimandrange/drivetrain.py
index 5d524e2b7..d411b7b8b 100644
--- a/photonlib-python-examples/aimandrange/drivetrain.py
+++ b/photonlib-python-examples/aimandrange/drivetrain.py
@@ -1,8 +1,27 @@
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -26,10 +45,10 @@ def __init__(self) -> None:
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.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.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)
@@ -97,7 +116,6 @@ def drive(
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
-
def updateOdometry(self) -> None:
"""Updates the field relative position of the robot."""
self.odometry.update(
@@ -111,29 +129,37 @@ def updateOdometry(self) -> None:
)
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
- return [
- self.frontLeft.getState(),
- self.frontRight.getState(),
- self.backLeft.getState(),
- self.backRight.getState(),
- ]
-
+ 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),
- ]
+ 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/"
@@ -145,12 +171,20 @@ def log(self):
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 + "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
+ )
- 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()
@@ -165,4 +199,4 @@ def simulationPeriodic(self):
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)
\ No newline at end of file
+ self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py
index 000782539..a175e4b4f 100644
--- a/photonlib-python-examples/aimandrange/robot.py
+++ b/photonlib-python-examples/aimandrange/robot.py
@@ -1,9 +1,27 @@
#!/usr/bin/env python3
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -18,10 +36,11 @@
VISION_STRAFE_kP = 0.5
VISION_DES_RANGE_m = 1.25
ROBOT_TO_CAM = wpimath.geometry.Transform3d(
- wpimath.geometry.Translation3d(0.5,0.0,0.5),
- wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0)
+ wpimath.geometry.Translation3d(0.5, 0.0, 0.5),
+ wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
)
+
class MyRobot(wpilib.TimedRobot):
def robotInit(self) -> None:
"""Robot initialization function"""
@@ -34,17 +53,17 @@ def robotPeriodic(self) -> None:
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
+ 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
targetRange = 0.0
targetVisible = False
results = self.cam.getAllUnreadResults()
- if(len(results) > 0):
- result = results[-1] #take the most recent result the camera had
+ if len(results) > 0:
+ result = results[-1] # take the most recent result the camera had
if result.hasTargets():
# At least one apriltag was seen by the camera
for target in result.getTargets():
@@ -52,22 +71,33 @@ def teleopPeriodic(self) -> None:
# Found tag 7, record its information
targetVisible = True
targetYaw = target.getYaw()
- heightDelta = (ROBOT_TO_CAM.translation().Z() -
- 1.435) # From 2024 game manual for ID 7
- angleDelta = -1.0 * ROBOT_TO_CAM.rotation().Y() - math.radians(target.getPitch())
+ heightDelta = (
+ ROBOT_TO_CAM.translation().Z() - 1.435
+ ) # From 2024 game manual for ID 7
+ angleDelta = -1.0 * ROBOT_TO_CAM.rotation().Y() - math.radians(
+ target.getPitch()
+ )
targetRange = heightDelta / math.tan(angleDelta)
- if(self.controller.getAButton() and targetVisible):
+ 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 and x-vel command with
- # an automatic one that turns toward the tag
+ # Override the driver's turn and x-vel command with
+ # an automatic one that turns toward the tag
# and puts us at the right distance
- rot = (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * drivetrain.kMaxAngularSpeed
- xSpeed = (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * drivetrain.kMaxSpeed
+ rot = (
+ (VISION_DES_ANGLE_deg - targetYaw)
+ * VISION_TURN_kP
+ * drivetrain.kMaxAngularSpeed
+ )
+ xSpeed = (
+ (VISION_DES_RANGE_m - targetRange)
+ * VISION_STRAFE_kP
+ * drivetrain.kMaxSpeed
+ )
self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod())
def _simulationPeriodic(self) -> None:
self.swerve.simulationPeriodic()
- return super()._simulationPeriodic()
\ No newline at end of file
+ return super()._simulationPeriodic()
diff --git a/photonlib-python-examples/aimandrange/swervemodule.py b/photonlib-python-examples/aimandrange/swervemodule.py
index c8c05e4ab..803a1f5f3 100644
--- a/photonlib-python-examples/aimandrange/swervemodule.py
+++ b/photonlib-python-examples/aimandrange/swervemodule.py
@@ -1,8 +1,26 @@
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -55,7 +73,7 @@ def __init__(
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)
+ 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)
@@ -82,12 +100,15 @@ def __init__(
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.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.
@@ -156,21 +177,37 @@ def log(self) -> None:
table = "Module " + str(self.moduleNumber) + "/"
wpilib.SmartDashboard.putNumber(
- table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians())))
+ table + "Steer Degrees",
+ math.degrees(wpimath.angleModulus(state.angle.radians())),
+ )
wpilib.SmartDashboard.putNumber(
- table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()))
+ 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 Feet", state.speed_fps)
+ table + "Drive Velocity Target Feet", self.desiredState.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)
-
+ 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)
+ 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
@@ -179,4 +216,3 @@ def simulationPeriodic(self) -> None:
self.simDriveEncoder.setRate(driveSpd)
self.simTurningEncoder.setDistance(self.simTurningEncoderPos)
self.simTurningEncoder.setRate(turnSpd)
-
diff --git a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
index 90ae6731e..e19514714 100644
--- a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
+++ b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
@@ -1,6 +1,6 @@
-'''
+"""
This test module imports tests that come with pyfrc, and can be used
to test basic functionality of just about any robot.
-'''
+"""
from pyfrc.tests import *
diff --git a/photonlib-python-examples/aimattarget/.gitignore b/photonlib-python-examples/aimattarget/.gitignore
index fa0d62106..244ad813e 100644
--- a/photonlib-python-examples/aimattarget/.gitignore
+++ b/photonlib-python-examples/aimattarget/.gitignore
@@ -172,4 +172,4 @@ out/
*-window.json
networktables.json
-__pycache__
\ No newline at end of file
+__pycache__
diff --git a/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json
index 2b42e34e6..f54f38af6 100644
--- a/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json
+++ b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json
@@ -1 +1 @@
-{"teamNumber": 1736}
\ No newline at end of file
+{"teamNumber": 1736}
diff --git a/photonlib-python-examples/aimattarget/drivetrain.py b/photonlib-python-examples/aimattarget/drivetrain.py
index 5d524e2b7..d411b7b8b 100644
--- a/photonlib-python-examples/aimattarget/drivetrain.py
+++ b/photonlib-python-examples/aimattarget/drivetrain.py
@@ -1,8 +1,27 @@
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -26,10 +45,10 @@ def __init__(self) -> None:
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.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.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)
@@ -97,7 +116,6 @@ def drive(
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
-
def updateOdometry(self) -> None:
"""Updates the field relative position of the robot."""
self.odometry.update(
@@ -111,29 +129,37 @@ def updateOdometry(self) -> None:
)
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
- return [
- self.frontLeft.getState(),
- self.frontRight.getState(),
- self.backLeft.getState(),
- self.backRight.getState(),
- ]
-
+ 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),
- ]
+ 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/"
@@ -145,12 +171,20 @@ def log(self):
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 + "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
+ )
- 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()
@@ -165,4 +199,4 @@ def simulationPeriodic(self):
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)
\ No newline at end of file
+ self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
diff --git a/photonlib-python-examples/aimattarget/robot.py b/photonlib-python-examples/aimattarget/robot.py
index 60ae2d89c..5266928c2 100644
--- a/photonlib-python-examples/aimattarget/robot.py
+++ b/photonlib-python-examples/aimattarget/robot.py
@@ -1,9 +1,28 @@
#!/usr/bin/env python3
+###################################################################################
+# MIT License
#
-# 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.
+# 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 wpilib
import drivetrain
@@ -12,6 +31,7 @@
VISION_TURN_kP = 0.01
+
class MyRobot(wpilib.TimedRobot):
def robotInit(self) -> None:
"""Robot initialization function"""
@@ -24,16 +44,16 @@ def robotPeriodic(self) -> None:
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
+ 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
+ if len(results) > 0:
+ result = results[-1] # take the most recent result the camera had
if result.hasTargets():
# At least one apriltag was seen by the camera
for target in result.getTargets():
@@ -42,7 +62,7 @@ def teleopPeriodic(self) -> None:
targetVisible = True
targetYaw = target.getYaw()
- if(self.controller.getAButton() and targetVisible):
+ 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.
@@ -52,4 +72,4 @@ def teleopPeriodic(self) -> None:
def _simulationPeriodic(self) -> None:
self.swerve.simulationPeriodic()
- return super()._simulationPeriodic()
\ No newline at end of file
+ return super()._simulationPeriodic()
diff --git a/photonlib-python-examples/aimattarget/swervemodule.py b/photonlib-python-examples/aimattarget/swervemodule.py
index c8c05e4ab..803a1f5f3 100644
--- a/photonlib-python-examples/aimattarget/swervemodule.py
+++ b/photonlib-python-examples/aimattarget/swervemodule.py
@@ -1,8 +1,26 @@
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -55,7 +73,7 @@ def __init__(
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)
+ 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)
@@ -82,12 +100,15 @@ def __init__(
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.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.
@@ -156,21 +177,37 @@ def log(self) -> None:
table = "Module " + str(self.moduleNumber) + "/"
wpilib.SmartDashboard.putNumber(
- table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians())))
+ table + "Steer Degrees",
+ math.degrees(wpimath.angleModulus(state.angle.radians())),
+ )
wpilib.SmartDashboard.putNumber(
- table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()))
+ 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 Feet", state.speed_fps)
+ table + "Drive Velocity Target Feet", self.desiredState.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)
-
+ 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)
+ 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
@@ -179,4 +216,3 @@ def simulationPeriodic(self) -> None:
self.simDriveEncoder.setRate(driveSpd)
self.simTurningEncoder.setDistance(self.simTurningEncoderPos)
self.simTurningEncoder.setRate(turnSpd)
-
diff --git a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
index 90ae6731e..e19514714 100644
--- a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
+++ b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
@@ -1,6 +1,6 @@
-'''
+"""
This test module imports tests that come with pyfrc, and can be used
to test basic functionality of just about any robot.
-'''
+"""
from pyfrc.tests import *
diff --git a/photonlib-python-examples/poseest/.gitignore b/photonlib-python-examples/poseest/.gitignore
index fa0d62106..244ad813e 100644
--- a/photonlib-python-examples/poseest/.gitignore
+++ b/photonlib-python-examples/poseest/.gitignore
@@ -172,4 +172,4 @@ out/
*-window.json
networktables.json
-__pycache__
\ No newline at end of file
+__pycache__
diff --git a/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json b/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json
index 2b42e34e6..f54f38af6 100644
--- a/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json
+++ b/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json
@@ -1 +1 @@
-{"teamNumber": 1736}
\ No newline at end of file
+{"teamNumber": 1736}
diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py
index 5d524e2b7..d411b7b8b 100644
--- a/photonlib-python-examples/poseest/drivetrain.py
+++ b/photonlib-python-examples/poseest/drivetrain.py
@@ -1,8 +1,27 @@
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -26,10 +45,10 @@ def __init__(self) -> None:
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.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.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)
@@ -97,7 +116,6 @@ def drive(
self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
-
def updateOdometry(self) -> None:
"""Updates the field relative position of the robot."""
self.odometry.update(
@@ -111,29 +129,37 @@ def updateOdometry(self) -> None:
)
def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
- return [
- self.frontLeft.getState(),
- self.frontRight.getState(),
- self.backLeft.getState(),
- self.backRight.getState(),
- ]
-
+ 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),
- ]
+ 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/"
@@ -145,12 +171,20 @@ def log(self):
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 + "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
+ )
- 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()
@@ -165,4 +199,4 @@ def simulationPeriodic(self):
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)
\ No newline at end of file
+ self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py
index cdf2f416c..bd476bde5 100644
--- a/photonlib-python-examples/poseest/robot.py
+++ b/photonlib-python-examples/poseest/robot.py
@@ -1,9 +1,28 @@
#!/usr/bin/env python3
+###################################################################################
+# MIT License
#
-# 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.
+# 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 wpilib
import drivetrain
@@ -20,12 +39,12 @@ def robotPeriodic(self) -> None:
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
+ xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed
+ ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed
+ rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed
self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod())
def _simulationPeriodic(self) -> None:
self.swerve.simulationPeriodic()
- return super()._simulationPeriodic()
\ No newline at end of file
+ return super()._simulationPeriodic()
diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py
index 2995fb6b9..02b9987fa 100644
--- a/photonlib-python-examples/poseest/swervemodule.py
+++ b/photonlib-python-examples/poseest/swervemodule.py
@@ -1,8 +1,26 @@
+###################################################################################
+# MIT License
#
-# 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.
+# 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
@@ -55,7 +73,7 @@ def __init__(
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)
+ 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)
@@ -81,12 +99,15 @@ def __init__(
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.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.
@@ -151,20 +172,34 @@ def log(self) -> None:
table = "Module " + str(self.moduleNumber) + "/"
wpilib.SmartDashboard.putNumber(
- table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians())))
+ 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 + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()))
+ table + "Drive Velocity Target Feet", self.desiredState.speed_fps
+ )
wpilib.SmartDashboard.putNumber(
- table + "Drive Velocity Feet", state.speed_fps)
+ table + "Drive Voltage", self.driveMotor.get() * 12.0
+ )
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)
-
+ 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)
+ 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 / 0.7
driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw)
turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw)
@@ -174,4 +209,3 @@ def simulationPeriodic(self) -> None:
self.simDriveEncoder.setRate(driveSpd)
self.simTurningEncoder.setDistance(self.simTurningEncoderPos)
self.simTurningEncoder.setRate(turnSpd)
-
diff --git a/photonlib-python-examples/poseest/tests/pyfrc_test.py b/photonlib-python-examples/poseest/tests/pyfrc_test.py
index 90ae6731e..e19514714 100644
--- a/photonlib-python-examples/poseest/tests/pyfrc_test.py
+++ b/photonlib-python-examples/poseest/tests/pyfrc_test.py
@@ -1,6 +1,6 @@
-'''
+"""
This test module imports tests that come with pyfrc, and can be used
to test basic functionality of just about any robot.
-'''
+"""
from pyfrc.tests import *