From 1da3519821d6f8e8549f597f99aad7e989a6a8a9 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sat, 7 Sep 2024 19:57:32 -0500 Subject: [PATCH] Python examples fixed up and added --- .../src/main/java/frc/robot/Robot.java | 4 +- .../aimandrange/.gitignore | 175 +++++++++++++++++ .../.wpilib/wpilib_preferences.json | 1 + .../aimandrange/drivetrain.py | 168 ++++++++++++++++ .../aimandrange/pyproject.toml | 33 ++++ .../aimandrange/robot.py | 73 +++++++ .../aimandrange/simgui-ds.json | 100 ++++++++++ .../aimandrange/simgui.json | 63 ++++++ .../aimandrange/swerve_module.png | Bin 0 -> 2954 bytes .../aimandrange/swervemodule.py | 182 ++++++++++++++++++ .../aimandrange/tests/pyfrc_test.py | 6 + .../aimattarget/.gitignore | 175 +++++++++++++++++ .../.wpilib/wpilib_preferences.json | 1 + .../aimattarget/drivetrain.py | 168 ++++++++++++++++ .../aimattarget/pyproject.toml | 33 ++++ .../aimattarget/robot.py | 55 ++++++ .../aimattarget/simgui-ds.json | 100 ++++++++++ .../aimattarget/simgui.json | 63 ++++++ .../aimattarget/swerve_module.png | Bin 0 -> 2954 bytes .../aimattarget/swervemodule.py | 182 ++++++++++++++++++ .../aimattarget/tests/pyfrc_test.py | 6 + .../poseest/drivetrain.py | 2 +- .../poseest/swervemodule.py | 9 +- 23 files changed, 1589 insertions(+), 10 deletions(-) create mode 100644 photonlib-python-examples/aimandrange/.gitignore create mode 100644 photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json create mode 100644 photonlib-python-examples/aimandrange/drivetrain.py create mode 100644 photonlib-python-examples/aimandrange/pyproject.toml create mode 100644 photonlib-python-examples/aimandrange/robot.py create mode 100644 photonlib-python-examples/aimandrange/simgui-ds.json create mode 100644 photonlib-python-examples/aimandrange/simgui.json create mode 100644 photonlib-python-examples/aimandrange/swerve_module.png create mode 100644 photonlib-python-examples/aimandrange/swervemodule.py create mode 100644 photonlib-python-examples/aimandrange/tests/pyfrc_test.py create mode 100644 photonlib-python-examples/aimattarget/.gitignore create mode 100644 photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json create mode 100644 photonlib-python-examples/aimattarget/drivetrain.py create mode 100644 photonlib-python-examples/aimattarget/pyproject.toml create mode 100644 photonlib-python-examples/aimattarget/robot.py create mode 100644 photonlib-python-examples/aimattarget/simgui-ds.json create mode 100644 photonlib-python-examples/aimattarget/simgui.json create mode 100644 photonlib-python-examples/aimattarget/swerve_module.png create mode 100644 photonlib-python-examples/aimattarget/swervemodule.py create mode 100644 photonlib-python-examples/aimattarget/tests/pyfrc_test.py diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java index 592fcb648a..28a54a2843 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java @@ -44,7 +44,7 @@ public class Robot extends TimedRobot { private PhotonCamera camera; private final double VISION_TURN_kP = 0.01; - private final double VISION_DES_ANGLE_rad = 0.0; + private final double VISION_DES_ANGLE_deg = 0.0; private final double VISION_STRAFE_kP = 0.5; private final double VISION_DES_RANGE_m = 1.25; @@ -121,7 +121,7 @@ public void teleopPeriodic() { // Override the driver's turn and fwd/rev command with an automatic one // That turns toward the tag, and gets the range right. turn = - (VISION_DES_ANGLE_rad - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; + (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; forward = (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed; } diff --git a/photonlib-python-examples/aimandrange/.gitignore b/photonlib-python-examples/aimandrange/.gitignore new file mode 100644 index 0000000000..fa0d62106f --- /dev/null +++ b/photonlib-python-examples/aimandrange/.gitignore @@ -0,0 +1,175 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +*-window.json +networktables.json + +__pycache__ \ No newline at end of file diff --git a/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..2b42e34e64 --- /dev/null +++ b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} \ No newline at end of file diff --git a/photonlib-python-examples/aimandrange/drivetrain.py b/photonlib-python-examples/aimandrange/drivetrain.py new file mode 100644 index 0000000000..5d524e2b75 --- /dev/null +++ b/photonlib-python-examples/aimandrange/drivetrain.py @@ -0,0 +1,168 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.geometry +import wpimath.kinematics +import swervemodule + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + + +class Drivetrain: + """ + Represents a swerve drive style drivetrain. + """ + + def __init__(self) -> None: + self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) + + self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) + self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) + self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11, 3) + self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15, 4) + + self.debugField = wpilib.Field2d() + wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField) + + self.gyro = wpilib.AnalogGyro(0) + self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro) + + self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( + self.frontLeftLocation, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.kinematics, + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + + self.gyro.reset() + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """ + Method to drive the robot using joystick info. + :param xSpeed: Speed of the robot in the x direction (forward). + :param ySpeed: Speed of the robot in the y direction (sideways). + :param rot: Angular rate of the robot. + :param fieldRelative: Whether the provided x and y speeds are relative to the field. + :param periodSeconds: Time + """ + swerveModuleStates = self.kinematics.toSwerveModuleStates( + wpimath.kinematics.ChassisSpeeds.discretize( + ( + wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, self.gyro.getRotation2d() + ) + if fieldRelative + else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot) + ), + periodSeconds, + ) + ) + wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, kMaxSpeed + ) + self.frontLeft.setDesiredState(swerveModuleStates[0]) + self.frontRight.setDesiredState(swerveModuleStates[1]) + self.backLeft.setDesiredState(swerveModuleStates[2]) + self.backRight.setDesiredState(swerveModuleStates[3]) + + self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates) + + + def updateOdometry(self) -> None: + """Updates the field relative position of the robot.""" + self.odometry.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + return [ + self.frontLeft.getState(), + self.frontRight.getState(), + self.backLeft.getState(), + self.backRight.getState(), + ] + + def getModulePoses(self) -> list[wpimath.geometry.Pose2d]: + p = self.odometry.getPose() + flTrans = wpimath.geometry.Transform2d(self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()) + frTrans = wpimath.geometry.Transform2d(self.frontRightLocation, self.frontRight.getAbsoluteHeading()) + blTrans = wpimath.geometry.Transform2d(self.backLeftLocation, self.backLeft.getAbsoluteHeading()) + brTrans = wpimath.geometry.Transform2d(self.backRightLocation, self.backRight.getAbsoluteHeading()) + return [ + p.transformBy(flTrans), + p.transformBy(frTrans), + p.transformBy(blTrans), + p.transformBy(brTrans), + ] + + def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds: + return self.kinematics.toChassisSpeeds(self.getModuleStates()) + + def log(self): + table = "Drive/" + + pose = self.odometry.getPose() + wpilib.SmartDashboard.putNumber(table + "X", pose.X()) + wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) + wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) + + chassisSpeeds = self.getChassisSpeeds() + wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Omega Degrees", chassisSpeeds.omega_dps) + + wpilib.SmartDashboard.putNumber(table + "Target VX", self.targetChassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "Target VY", self.targetChassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps) + + self.frontLeft.log() + self.frontRight.log() + self.backLeft.log() + self.backRight.log() + + self.debugField.getRobotObject().setPose(self.odometry.getPose()) + self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) + + def simulationPeriodic(self): + self.frontLeft.simulationPeriodic() + self.frontRight.simulationPeriodic() + self.backLeft.simulationPeriodic() + self.backRight.simulationPeriodic() + self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps) + self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) \ No newline at end of file diff --git a/photonlib-python-examples/aimandrange/pyproject.toml b/photonlib-python-examples/aimandrange/pyproject.toml new file mode 100644 index 0000000000..07d7ac9694 --- /dev/null +++ b/photonlib-python-examples/aimandrange/pyproject.toml @@ -0,0 +1,33 @@ +# +# Use this configuration file to control what RobotPy packages are installed +# on your RoboRIO +# + +[tool.robotpy] + +# Version of robotpy this project depends on +robotpy_version = "2024.3.2.2" + +# Which extra RobotPy components should be installed +# -> equivalent to `pip install robotpy[extra1, ...] +robotpy_extras = [ + # "all", + # "apriltag", + # "commands2", + # "cscore", + # "navx", + # "pathplannerlib", + # "phoenix5", + # "phoenix6", + # "photonvision", + # "playingwithfusion", + # "rev", + # "romi", + # "sim", + # "xrp", +] + +# Other pip packages to install +requires = [ + "photonlibpy" +] diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py new file mode 100644 index 0000000000..0007825395 --- /dev/null +++ b/photonlib-python-examples/aimandrange/robot.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpimath +import wpimath.geometry +import drivetrain + +from photonlibpy import PhotonCamera + +VISION_TURN_kP = 0.01 +VISION_DES_ANGLE_deg = 0.0 +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) +) + +class MyRobot(wpilib.TimedRobot): + def robotInit(self) -> None: + """Robot initialization function""" + self.controller = wpilib.XboxController(0) + self.swerve = drivetrain.Drivetrain() + self.cam = PhotonCamera("YOUR CAMERA NAME") + + def robotPeriodic(self) -> None: + self.swerve.updateOdometry() + self.swerve.log() + + def teleopPeriodic(self) -> None: + xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed + ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed + rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed + + # Get information from the camera + targetYaw = 0.0 + 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 result.hasTargets(): + # At least one apriltag was seen by the camera + for target in result.getTargets(): + if target.getFiducialId() == 7: + # 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()) + targetRange = heightDelta / math.tan(angleDelta) + + 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 + # 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 + + 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 diff --git a/photonlib-python-examples/aimandrange/simgui-ds.json b/photonlib-python-examples/aimandrange/simgui-ds.json new file mode 100644 index 0000000000..addd5860ce --- /dev/null +++ b/photonlib-python-examples/aimandrange/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decayRate": 0.0, + "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 + } + ], + "axisCount": 6, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/photonlib-python-examples/aimandrange/simgui.json b/photonlib-python-examples/aimandrange/simgui.json new file mode 100644 index 0000000000..2a9feb9f77 --- /dev/null +++ b/photonlib-python-examples/aimandrange/simgui.json @@ -0,0 +1,63 @@ +{ + "HALProvider": { + "Encoders": { + "0": { + "header": { + "open": true + } + }, + "window": { + "visible": true + } + }, + "Other Devices": { + "AnalogGyro[0]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Drivetrain Debug": "Field2d" + }, + "windows": { + "/SmartDashboard/Drivetrain Debug": { + "SwerveModules": { + "image": "swerve_module.png", + "length": 0.5, + "width": 0.5 + }, + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drivetrain Debug": { + "double[]##v_/SmartDashboard/Drivetrain Debug/Robot": { + "open": true + } + }, + "Module 1": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/photonlib-python-examples/aimandrange/swerve_module.png b/photonlib-python-examples/aimandrange/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-python-examples/aimandrange/swervemodule.py b/photonlib-python-examples/aimandrange/swervemodule.py new file mode 100644 index 0000000000..c8c05e4aba --- /dev/null +++ b/photonlib-python-examples/aimandrange/swervemodule.py @@ -0,0 +1,182 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.kinematics +import wpimath.filter +import wpimath.geometry +import wpimath.controller +import wpimath.trajectory +import wpimath.units + +kWheelRadius = 0.0508 +kEncoderResolution = 4096 +kModuleMaxAngularVelocity = math.pi +kModuleMaxAngularAcceleration = math.tau + + +class SwerveModule: + def __init__( + self, + driveMotorChannel: int, + turningMotorChannel: int, + driveEncoderChannelA: int, + driveEncoderChannelB: int, + turningEncoderChannelA: int, + turningEncoderChannelB: int, + moduleNumber: int, + ) -> None: + """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder. + + :param driveMotorChannel: PWM output for the drive motor. + :param turningMotorChannel: PWM output for the turning motor. + :param driveEncoderChannelA: DIO input for the drive encoder channel A + :param driveEncoderChannelB: DIO input for the drive encoder channel B + :param turningEncoderChannelA: DIO input for the turning encoder channel A + :param turningEncoderChannelB: DIO input for the turning encoder channel B + """ + self.moduleNumber = moduleNumber + self.desiredState = wpimath.kinematics.SwerveModuleState() + + self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) + self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) + + self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB) + self.turningEncoder = wpilib.Encoder( + turningEncoderChannelA, turningEncoderChannelB + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.drivePIDController = wpimath.controller.PIDController(10, 0, 0) + + # Gains are for example purposes only - must be determined for your own robot! + self.turningPIDController = wpimath.controller.PIDController(30,0,0) + + # Gains are for example purposes only - must be determined for your own robot! + self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7) + + # Set the distance per pulse for the drive encoder. We can simply use the + # distance traveled for one rotation of the wheel divided by the encoder + # resolution. + self.driveEncoder.setDistancePerPulse( + math.tau * kWheelRadius / kEncoderResolution + ) + + # Set the distance (in this case, angle) in radians per pulse for the turning encoder. + # This is the the angle through an entire rotation (2 * pi) divided by the + # encoder resolution. + self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution) + + # Limit the PID Controller's input range between -pi and pi and set the input + # to be continuous. + self.turningPIDController.enableContinuousInput(-math.pi, math.pi) + + # Simulation Support + self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder) + self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder) + self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor) + self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor) + self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(0.1, 0.02) + self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(0.0001, 0.02) + self.simTurningEncoderPos = 0 + self.simDrivingEncoderPos = 0 + + + def getState(self) -> wpimath.kinematics.SwerveModuleState: + """Returns the current state of the module. + + :returns: The current state of the module. + """ + return wpimath.kinematics.SwerveModuleState( + self.driveEncoder.getRate(), + wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + ) + + def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + """Returns the current position of the module. + + :returns: The current position of the module. + """ + return wpimath.kinematics.SwerveModulePosition( + self.driveEncoder.getDistance(), + wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + ) + + def setDesiredState( + self, desiredState: wpimath.kinematics.SwerveModuleState + ) -> None: + """Sets the desired state for the module. + + :param desiredState: Desired state with speed and angle. + """ + self.desiredState = desiredState + + encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + + # Optimize the reference state to avoid spinning further than 90 degrees + state = wpimath.kinematics.SwerveModuleState.optimize( + self.desiredState, encoderRotation + ) + + # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired + # direction of travel that can occur when modules change directions. This results in smoother + # driving. + state.speed *= (state.angle - encoderRotation).cos() + + # Calculate the drive output from the drive PID controller. + driveOutput = self.drivePIDController.calculate( + self.driveEncoder.getRate(), state.speed + ) + + driveFeedforward = self.driveFeedforward.calculate(state.speed) + + # Calculate the turning motor output from the turning PID controller. + turnOutput = self.turningPIDController.calculate( + self.turningEncoder.getDistance(), state.angle.radians() + ) + + turnFeedforward = self.turnFeedforward.calculate( + self.turningPIDController.getSetpoint() + ) + + self.driveMotor.setVoltage(driveOutput + driveFeedforward) + self.turningMotor.setVoltage(turnOutput + turnFeedforward) + + def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: + return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + + def log(self) -> None: + state = self.getState() + + table = "Module " + str(self.moduleNumber) + "/" + wpilib.SmartDashboard.putNumber( + table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians()))) + wpilib.SmartDashboard.putNumber( + table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint())) + wpilib.SmartDashboard.putNumber( + table + "Drive Velocity Feet", state.speed_fps) + wpilib.SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", self.desiredState.speed_fps) + wpilib.SmartDashboard.putNumber(table + "Drive Voltage", self.driveMotor.get() * 12.0) + wpilib.SmartDashboard.putNumber(table + "Steer Voltage", self.turningMotor.get() * 12.0) + + def simulationPeriodic(self) -> None: + driveVoltage = self.simDrivingMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage() + turnVoltage = self.simTurningMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage() + driveSpdRaw = driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0,0) + turnSpdRaw = turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0,0) + driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw) + turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw) + self.simDrivingEncoderPos += 0.02 * driveSpd + self.simTurningEncoderPos += 0.02 * turnSpd + self.simDriveEncoder.setDistance(self.simDrivingEncoderPos) + self.simDriveEncoder.setRate(driveSpd) + self.simTurningEncoder.setDistance(self.simTurningEncoderPos) + self.simTurningEncoder.setRate(turnSpd) + diff --git a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py new file mode 100644 index 0000000000..90ae6731e4 --- /dev/null +++ b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py @@ -0,0 +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 new file mode 100644 index 0000000000..fa0d62106f --- /dev/null +++ b/photonlib-python-examples/aimattarget/.gitignore @@ -0,0 +1,175 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +*-window.json +networktables.json + +__pycache__ \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..2b42e34e64 --- /dev/null +++ b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/drivetrain.py b/photonlib-python-examples/aimattarget/drivetrain.py new file mode 100644 index 0000000000..5d524e2b75 --- /dev/null +++ b/photonlib-python-examples/aimattarget/drivetrain.py @@ -0,0 +1,168 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.geometry +import wpimath.kinematics +import swervemodule + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + + +class Drivetrain: + """ + Represents a swerve drive style drivetrain. + """ + + def __init__(self) -> None: + self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) + + self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1) + self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2) + self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11, 3) + self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15, 4) + + self.debugField = wpilib.Field2d() + wpilib.SmartDashboard.putData("Drivetrain Debug", self.debugField) + + self.gyro = wpilib.AnalogGyro(0) + self.simGyro = wpilib.simulation.AnalogGyroSim(self.gyro) + + self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( + self.frontLeftLocation, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.kinematics, + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() + + self.gyro.reset() + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """ + Method to drive the robot using joystick info. + :param xSpeed: Speed of the robot in the x direction (forward). + :param ySpeed: Speed of the robot in the y direction (sideways). + :param rot: Angular rate of the robot. + :param fieldRelative: Whether the provided x and y speeds are relative to the field. + :param periodSeconds: Time + """ + swerveModuleStates = self.kinematics.toSwerveModuleStates( + wpimath.kinematics.ChassisSpeeds.discretize( + ( + wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, self.gyro.getRotation2d() + ) + if fieldRelative + else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot) + ), + periodSeconds, + ) + ) + wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, kMaxSpeed + ) + self.frontLeft.setDesiredState(swerveModuleStates[0]) + self.frontRight.setDesiredState(swerveModuleStates[1]) + self.backLeft.setDesiredState(swerveModuleStates[2]) + self.backRight.setDesiredState(swerveModuleStates[3]) + + self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates) + + + def updateOdometry(self) -> None: + """Updates the field relative position of the robot.""" + self.odometry.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: + return [ + self.frontLeft.getState(), + self.frontRight.getState(), + self.backLeft.getState(), + self.backRight.getState(), + ] + + def getModulePoses(self) -> list[wpimath.geometry.Pose2d]: + p = self.odometry.getPose() + flTrans = wpimath.geometry.Transform2d(self.frontLeftLocation, self.frontLeft.getAbsoluteHeading()) + frTrans = wpimath.geometry.Transform2d(self.frontRightLocation, self.frontRight.getAbsoluteHeading()) + blTrans = wpimath.geometry.Transform2d(self.backLeftLocation, self.backLeft.getAbsoluteHeading()) + brTrans = wpimath.geometry.Transform2d(self.backRightLocation, self.backRight.getAbsoluteHeading()) + return [ + p.transformBy(flTrans), + p.transformBy(frTrans), + p.transformBy(blTrans), + p.transformBy(brTrans), + ] + + def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds: + return self.kinematics.toChassisSpeeds(self.getModuleStates()) + + def log(self): + table = "Drive/" + + pose = self.odometry.getPose() + wpilib.SmartDashboard.putNumber(table + "X", pose.X()) + wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) + wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) + + chassisSpeeds = self.getChassisSpeeds() + wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Omega Degrees", chassisSpeeds.omega_dps) + + wpilib.SmartDashboard.putNumber(table + "Target VX", self.targetChassisSpeeds.vx) + wpilib.SmartDashboard.putNumber(table + "Target VY", self.targetChassisSpeeds.vy) + wpilib.SmartDashboard.putNumber(table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps) + + self.frontLeft.log() + self.frontRight.log() + self.backLeft.log() + self.backRight.log() + + self.debugField.getRobotObject().setPose(self.odometry.getPose()) + self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) + + def simulationPeriodic(self): + self.frontLeft.simulationPeriodic() + self.frontRight.simulationPeriodic() + self.backLeft.simulationPeriodic() + self.backRight.simulationPeriodic() + self.simGyro.setRate(-1.0 * self.getChassisSpeeds().omega_dps) + self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02) \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/pyproject.toml b/photonlib-python-examples/aimattarget/pyproject.toml new file mode 100644 index 0000000000..07d7ac9694 --- /dev/null +++ b/photonlib-python-examples/aimattarget/pyproject.toml @@ -0,0 +1,33 @@ +# +# Use this configuration file to control what RobotPy packages are installed +# on your RoboRIO +# + +[tool.robotpy] + +# Version of robotpy this project depends on +robotpy_version = "2024.3.2.2" + +# Which extra RobotPy components should be installed +# -> equivalent to `pip install robotpy[extra1, ...] +robotpy_extras = [ + # "all", + # "apriltag", + # "commands2", + # "cscore", + # "navx", + # "pathplannerlib", + # "phoenix5", + # "phoenix6", + # "photonvision", + # "playingwithfusion", + # "rev", + # "romi", + # "sim", + # "xrp", +] + +# Other pip packages to install +requires = [ + "photonlibpy" +] diff --git a/photonlib-python-examples/aimattarget/robot.py b/photonlib-python-examples/aimattarget/robot.py new file mode 100644 index 0000000000..60ae2d89c3 --- /dev/null +++ b/photonlib-python-examples/aimattarget/robot.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import wpilib +import drivetrain + +from photonlibpy import PhotonCamera + +VISION_TURN_kP = 0.01 + +class MyRobot(wpilib.TimedRobot): + def robotInit(self) -> None: + """Robot initialization function""" + self.controller = wpilib.XboxController(0) + self.swerve = drivetrain.Drivetrain() + self.cam = PhotonCamera("YOUR CAMERA NAME") + + def robotPeriodic(self) -> None: + self.swerve.updateOdometry() + self.swerve.log() + + def teleopPeriodic(self) -> None: + xSpeed = -1.0 * self.controller.getLeftY() * drivetrain.kMaxSpeed + ySpeed = -1.0 * self.controller.getLeftX() * drivetrain.kMaxSpeed + rot = -1.0 * self.controller.getRightX() * drivetrain.kMaxAngularSpeed + + # Get information from the camera + targetYaw = 0.0 + targetVisible = False + results = self.cam.getAllUnreadResults() + if(len(results) > 0): + result = results[-1] #take the most recent result the camera had + if result.hasTargets(): + # At least one apriltag was seen by the camera + for target in result.getTargets(): + if target.getFiducialId() == 7: + # Found tag 7, record its information + targetVisible = True + targetYaw = target.getYaw() + + if(self.controller.getAButton() and targetVisible): + # Driver wants auto-alignment to tag 7 + # And, tag 7 is in sight, so we can turn toward it. + # Override the driver's turn command with an automatic one that turns toward the tag. + rot = -1.0 * targetYaw * VISION_TURN_kP * drivetrain.kMaxAngularSpeed + + self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod()) + + def _simulationPeriodic(self) -> None: + self.swerve.simulationPeriodic() + return super()._simulationPeriodic() \ No newline at end of file diff --git a/photonlib-python-examples/aimattarget/simgui-ds.json b/photonlib-python-examples/aimattarget/simgui-ds.json new file mode 100644 index 0000000000..addd5860ce --- /dev/null +++ b/photonlib-python-examples/aimattarget/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decayRate": 0.0, + "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 + } + ], + "axisCount": 6, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/photonlib-python-examples/aimattarget/simgui.json b/photonlib-python-examples/aimattarget/simgui.json new file mode 100644 index 0000000000..2a9feb9f77 --- /dev/null +++ b/photonlib-python-examples/aimattarget/simgui.json @@ -0,0 +1,63 @@ +{ + "HALProvider": { + "Encoders": { + "0": { + "header": { + "open": true + } + }, + "window": { + "visible": true + } + }, + "Other Devices": { + "AnalogGyro[0]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Drivetrain Debug": "Field2d" + }, + "windows": { + "/SmartDashboard/Drivetrain Debug": { + "SwerveModules": { + "image": "swerve_module.png", + "length": 0.5, + "width": 0.5 + }, + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drivetrain Debug": { + "double[]##v_/SmartDashboard/Drivetrain Debug/Robot": { + "open": true + } + }, + "Module 1": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/photonlib-python-examples/aimattarget/swerve_module.png b/photonlib-python-examples/aimattarget/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-python-examples/aimattarget/swervemodule.py b/photonlib-python-examples/aimattarget/swervemodule.py new file mode 100644 index 0000000000..c8c05e4aba --- /dev/null +++ b/photonlib-python-examples/aimattarget/swervemodule.py @@ -0,0 +1,182 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import math +import wpilib +import wpilib.simulation +import wpimath.kinematics +import wpimath.filter +import wpimath.geometry +import wpimath.controller +import wpimath.trajectory +import wpimath.units + +kWheelRadius = 0.0508 +kEncoderResolution = 4096 +kModuleMaxAngularVelocity = math.pi +kModuleMaxAngularAcceleration = math.tau + + +class SwerveModule: + def __init__( + self, + driveMotorChannel: int, + turningMotorChannel: int, + driveEncoderChannelA: int, + driveEncoderChannelB: int, + turningEncoderChannelA: int, + turningEncoderChannelB: int, + moduleNumber: int, + ) -> None: + """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder. + + :param driveMotorChannel: PWM output for the drive motor. + :param turningMotorChannel: PWM output for the turning motor. + :param driveEncoderChannelA: DIO input for the drive encoder channel A + :param driveEncoderChannelB: DIO input for the drive encoder channel B + :param turningEncoderChannelA: DIO input for the turning encoder channel A + :param turningEncoderChannelB: DIO input for the turning encoder channel B + """ + self.moduleNumber = moduleNumber + self.desiredState = wpimath.kinematics.SwerveModuleState() + + self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) + self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) + + self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB) + self.turningEncoder = wpilib.Encoder( + turningEncoderChannelA, turningEncoderChannelB + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.drivePIDController = wpimath.controller.PIDController(10, 0, 0) + + # Gains are for example purposes only - must be determined for your own robot! + self.turningPIDController = wpimath.controller.PIDController(30,0,0) + + # Gains are for example purposes only - must be determined for your own robot! + self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7) + + # Set the distance per pulse for the drive encoder. We can simply use the + # distance traveled for one rotation of the wheel divided by the encoder + # resolution. + self.driveEncoder.setDistancePerPulse( + math.tau * kWheelRadius / kEncoderResolution + ) + + # Set the distance (in this case, angle) in radians per pulse for the turning encoder. + # This is the the angle through an entire rotation (2 * pi) divided by the + # encoder resolution. + self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution) + + # Limit the PID Controller's input range between -pi and pi and set the input + # to be continuous. + self.turningPIDController.enableContinuousInput(-math.pi, math.pi) + + # Simulation Support + self.simDriveEncoder = wpilib.simulation.EncoderSim(self.driveEncoder) + self.simTurningEncoder = wpilib.simulation.EncoderSim(self.turningEncoder) + self.simDrivingMotor = wpilib.simulation.PWMSim(self.driveMotor) + self.simTurningMotor = wpilib.simulation.PWMSim(self.turningMotor) + self.simDrivingMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(0.1, 0.02) + self.simTurningMotorFilter = wpimath.filter.LinearFilter.singlePoleIIR(0.0001, 0.02) + self.simTurningEncoderPos = 0 + self.simDrivingEncoderPos = 0 + + + def getState(self) -> wpimath.kinematics.SwerveModuleState: + """Returns the current state of the module. + + :returns: The current state of the module. + """ + return wpimath.kinematics.SwerveModuleState( + self.driveEncoder.getRate(), + wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + ) + + def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + """Returns the current position of the module. + + :returns: The current position of the module. + """ + return wpimath.kinematics.SwerveModulePosition( + self.driveEncoder.getDistance(), + wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + ) + + def setDesiredState( + self, desiredState: wpimath.kinematics.SwerveModuleState + ) -> None: + """Sets the desired state for the module. + + :param desiredState: Desired state with speed and angle. + """ + self.desiredState = desiredState + + encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + + # Optimize the reference state to avoid spinning further than 90 degrees + state = wpimath.kinematics.SwerveModuleState.optimize( + self.desiredState, encoderRotation + ) + + # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired + # direction of travel that can occur when modules change directions. This results in smoother + # driving. + state.speed *= (state.angle - encoderRotation).cos() + + # Calculate the drive output from the drive PID controller. + driveOutput = self.drivePIDController.calculate( + self.driveEncoder.getRate(), state.speed + ) + + driveFeedforward = self.driveFeedforward.calculate(state.speed) + + # Calculate the turning motor output from the turning PID controller. + turnOutput = self.turningPIDController.calculate( + self.turningEncoder.getDistance(), state.angle.radians() + ) + + turnFeedforward = self.turnFeedforward.calculate( + self.turningPIDController.getSetpoint() + ) + + self.driveMotor.setVoltage(driveOutput + driveFeedforward) + self.turningMotor.setVoltage(turnOutput + turnFeedforward) + + def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: + return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + + def log(self) -> None: + state = self.getState() + + table = "Module " + str(self.moduleNumber) + "/" + wpilib.SmartDashboard.putNumber( + table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians()))) + wpilib.SmartDashboard.putNumber( + table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint())) + wpilib.SmartDashboard.putNumber( + table + "Drive Velocity Feet", state.speed_fps) + wpilib.SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", self.desiredState.speed_fps) + wpilib.SmartDashboard.putNumber(table + "Drive Voltage", self.driveMotor.get() * 12.0) + wpilib.SmartDashboard.putNumber(table + "Steer Voltage", self.turningMotor.get() * 12.0) + + def simulationPeriodic(self) -> None: + driveVoltage = self.simDrivingMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage() + turnVoltage = self.simTurningMotor.getSpeed() * wpilib.RobotController.getBatteryVoltage() + driveSpdRaw = driveVoltage / 12.0 * self.driveFeedforward.maxAchievableVelocity(12.0,0) + turnSpdRaw = turnVoltage / 12.0 * self.turnFeedforward.maxAchievableVelocity(12.0,0) + driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw) + turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw) + self.simDrivingEncoderPos += 0.02 * driveSpd + self.simTurningEncoderPos += 0.02 * turnSpd + self.simDriveEncoder.setDistance(self.simDrivingEncoderPos) + self.simDriveEncoder.setRate(driveSpd) + self.simTurningEncoder.setDistance(self.simTurningEncoderPos) + self.simTurningEncoder.setRate(turnSpd) + diff --git a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py new file mode 100644 index 0000000000..90ae6731e4 --- /dev/null +++ b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py @@ -0,0 +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/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py index 25be5d19e9..5d524e2b75 100644 --- a/photonlib-python-examples/poseest/drivetrain.py +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -164,5 +164,5 @@ def simulationPeriodic(self): self.frontRight.simulationPeriodic() self.backLeft.simulationPeriodic() self.backRight.simulationPeriodic() - self.simGyro.setRate(self.getChassisSpeeds().omega_dps) + 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 diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py index c8c05e4aba..2995fb6b9a 100644 --- a/photonlib-python-examples/poseest/swervemodule.py +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -59,7 +59,6 @@ def __init__( # Gains are for example purposes only - must be determined for your own robot! self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) - self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7) # Set the distance per pulse for the drive encoder. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder @@ -141,12 +140,8 @@ def setDesiredState( self.turningEncoder.getDistance(), state.angle.radians() ) - turnFeedforward = self.turnFeedforward.calculate( - self.turningPIDController.getSetpoint() - ) - self.driveMotor.setVoltage(driveOutput + driveFeedforward) - self.turningMotor.setVoltage(turnOutput + turnFeedforward) + self.turningMotor.setVoltage(turnOutput) def getAbsoluteHeading(self) -> wpimath.geometry.Rotation2d: return wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) @@ -170,7 +165,7 @@ 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) + turnSpdRaw = turnVoltage / 0.7 driveSpd = self.simDrivingMotorFilter.calculate(driveSpdRaw) turnSpd = self.simTurningMotorFilter.calculate(turnSpdRaw) self.simDrivingEncoderPos += 0.02 * driveSpd