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 *