diff --git a/docs/source/docs/additional-resources/best-practices.md b/docs/source/docs/additional-resources/best-practices.md index b45ed0562..7dd11ecd0 100644 --- a/docs/source/docs/additional-resources/best-practices.md +++ b/docs/source/docs/additional-resources/best-practices.md @@ -15,9 +15,9 @@ - Make sure you take advantage of the field calibration time given at the start of the event: - Bring your robot to the field at the allotted time. - Turn on your robot and pull up the dashboard on your driver station. - - Point your robot at the target(s) and ensure you get a consistent tracking (you hold one target consistently, the ceiling lights aren't detected, etc.). - - If you have problems with your pipeline, go to the pipeline tuning section and retune the pipeline using the guide there. You want to make your exposure as low as possible with a tight hue value to ensure no extra targets are detected. - - Move the robot close, far, angled, and around the field to ensure no extra targets are found anywhere when looking for a target. + - Point your robot at the AprilTags(s) and ensure you get a consistent tracking (you hold one AprilTag consistently, the ceiling lights aren't detected, etc.). + - If you have problems with your pipeline, go to the pipeline tuning section and retune the pipeline using the guide there. + - Move the robot close, far, angled, and around the field to ensure no extra AprilTags are found. - Go to a practice match to ensure everything is working correctly. - After field calibration, use the "Export Settings" button in the "Settings" page to create a backup. - Do this for each coprocessor on your robot that runs PhotonVision, and name your exports with meaningful names. @@ -26,4 +26,4 @@ - This effectively works as a snapshot of your PhotonVision data that can be restored at any point. - Before every match, check the ethernet connection going into your coprocessor and that it is seated fully. - Ensure that exposure is as low as possible and that you don't have the dashboard up when you don't need it to reduce bandwidth. -- Stream at as low of a resolution as possible while still detecting targets to stay within bandwidth limits. +- Stream at as low of a resolution as possible while still detecting AprilTags to stay within field bandwidth limits. diff --git a/docs/source/docs/apriltag-pipelines/multitag.md b/docs/source/docs/apriltag-pipelines/multitag.md index 102b36ce1..da5169fb0 100644 --- a/docs/source/docs/apriltag-pipelines/multitag.md +++ b/docs/source/docs/apriltag-pipelines/multitag.md @@ -24,7 +24,7 @@ This multi-target pose estimate can be accessed using PhotonLib. We suggest usin ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java var result = camera.getLatestResult(); if (result.getMultiTagResult().estimatedPose.isPresent) { @@ -38,6 +38,11 @@ This multi-target pose estimate can be accessed using PhotonLib. We suggest usin if (result.MultiTagResult().result.isPresent) { frc::Transform3d fieldToCamera = result.MultiTagResult().result.best; } + + .. code-block:: Python + + # Coming Soon! + ``` :::{note} diff --git a/docs/source/docs/contributing/building-photon.md b/docs/source/docs/contributing/building-photon.md index 8f2fb8c18..7eb7b8a99 100644 --- a/docs/source/docs/contributing/building-photon.md +++ b/docs/source/docs/contributing/building-photon.md @@ -243,19 +243,11 @@ The program will wait for the VSCode debugger to attach before proceeding. ### Running examples -You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the photonlib-java-examples and photonlib-cpp-examples subdirectories, respectively. The projects currently available include: +You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the photonlib-*-examples subdirectories for each language. -- photonlib-java-examples: - - aimandrange:simulateJava - - aimattarget:simulateJava - - getinrange:simulateJava - - simaimandrange:simulateJava - - simposeest:simulateJava -- photonlib-cpp-examples: - - aimandrange:simulateNative - - getinrange:simulateNative +#### Running C++/Java -To run them, use the commands listed below. PhotonLib must first be published to your local maven repository, then the copy PhotonLib task will copy the generated vendordep json file into each example. After that, the simulateJava/simulateNative task can be used like a normal robot project. Robot simulation with attached debugger is technically possible by using simulateExternalJava and modifying the launch script it exports, though unsupported. +PhotonLib must first be published to your local maven repository, then the copy PhotonLib task will copy the generated vendordep json file into each example. After that, the simulateJava/simulateNative task can be used like a normal robot project. Robot simulation with attached debugger is technically possible by using simulateExternalJava and modifying the launch script it exports, though not yet supported. ``` ~/photonvision$ ./gradlew publishToMavenLocal @@ -268,3 +260,27 @@ To run them, use the commands listed below. PhotonLib must first be published to ~/photonvision/photonlib-cpp-examples$ ./gradlew copyPhotonlib ~/photonvision/photonlib-cpp-examples$ ./gradlew :simulateNative ``` + +#### Running Python + +PhotonLibPy must first be built into a wheel. + +``` +> cd photon-lib/py +> buildAndTest.bat +``` + +Then, you must enable using the development wheels. robotpy will use pip behind the scenes, and this bat file tells pip about your development artifacts. + +Note: This is best done in a virtual environment. + +``` +> enableUsingDevBuilds.bat +``` + +Then, run the examples: + +``` +> cd photonlib-python-examples +> run.bat +``` diff --git a/docs/source/docs/description.md b/docs/source/docs/description.md index ad168b3c7..2a962188e 100644 --- a/docs/source/docs/description.md +++ b/docs/source/docs/description.md @@ -3,7 +3,7 @@ ## Description PhotonVision is a free, fast, and easy-to-use vision processing solution for the *FIRST*Robotics Competition. PhotonVision is designed to get vision working on your robot *quickly*, without the significant cost of other similar solutions. -Using PhotonVision, teams can go from setting up a camera and coprocessor to detecting and tracking targets by simply tuning sliders. With an easy to use interface, comprehensive documentation, and a feature rich vendor dependency, no experience is necessary to use PhotonVision. No matter your resources, using PhotonVision is easy compared to its alternatives. +Using PhotonVision, teams can go from setting up a camera and coprocessor to detecting and tracking AprilTags and other targets by simply tuning sliders. With an easy to use interface, comprehensive documentation, and a feature rich vendor dependency, no experience is necessary to use PhotonVision. No matter your resources, using PhotonVision is easy compared to its alternatives. ## Advantages diff --git a/docs/source/docs/examples/aimandrange.md b/docs/source/docs/examples/aimandrange.md index f50a71188..556da387b 100644 --- a/docs/source/docs/examples/aimandrange.md +++ b/docs/source/docs/examples/aimandrange.md @@ -4,36 +4,47 @@ The following example is from the PhotonLib example repository ([Java](https://g ## Knowledge and Equipment Needed -- Everything required in {ref}`Aiming at a Target ` and {ref}`Getting in Range of the Target `. +- Everything required in {ref}`Aiming at a Target `. ## Code -Now that you know how to both aim and get in range of the target, it is time to combine them both at the same time. This example will take the previous two code examples and make them into one function using the same tools as before. With this example, you now have all the knowledge you need to use PhotonVision on your robot in any game. +Now that you know how to aim toward the AprilTag, let's also drive the correct distance from the AprilTag. + +To do this, we'll use the *pitch* of the target in the camera image and trigonometry to figure out how far away the robot is from the AprilTag. Then, like before, we'll use the P term of a PID controller to drive the robot to the correct distance. ```{eval-rst} .. tab-set:: .. tab-item:: Java - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java :language: java - :lines: 42-111 + :lines: 84-131 :linenos: - :lineno-start: 42 + :lineno-start: 84 .. tab-item:: C++ (Header) - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h - :language: cpp - :lines: 27-71 + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h + :language: c++ + :lines: 25-63 :linenos: - :lineno-start: 27 + :lineno-start: 25 .. tab-item:: C++ (Source) - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp - :language: cpp - :lines: 25-67 + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp + :language: c++ + :lines: 58-107 :linenos: - :lineno-start: 25 + :lineno-start: 58 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-python-examples/aimandrange/robot.py + :language: python + :lines: 44-98 + :linenos: + :lineno-start: 44 + ``` diff --git a/docs/source/docs/examples/aimingatatarget.md b/docs/source/docs/examples/aimingatatarget.md index d0d4bd394..b275baefa 100644 --- a/docs/source/docs/examples/aimingatatarget.md +++ b/docs/source/docs/examples/aimingatatarget.md @@ -1,45 +1,55 @@ # Aiming at a Target -The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/master/photonlib-java-examples/aimattarget)/[C++](https://github.com/PhotonVision/photonvision/tree/master/photonlib-cpp-examples/aimattarget)). +The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/master/photonlib-java-examples/aimattarget)). ## Knowledge and Equipment Needed -- Robot with a vision system running PhotonVision -- Target -- Ability to track a target by properly tuning a pipeline +- A Robot +- A camera mounted rigidly to the robot's frame, cenetered and pointed forward. +- A coprocessor running PhotonVision with an AprilTag or Aurco 2D Pipeline. +- [A printout of Apriltag 7](https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/Apriltag_Images_and_User_Guide.pdf), mounted on a rigid and flat surface. ## Code -Now that you have properly set up your vision system and have tuned a pipeline, you can now aim your robot/turret at the target using the data from PhotonVision. This data is reported over NetworkTables and includes: latency, whether there is a target detected or not, pitch, yaw, area, skew, and target pose relative to the robot. This data will be used/manipulated by our vendor dependency, PhotonLib. The documentation for the Network Tables API can be found {ref}`here ` and the documentation for PhotonLib {ref}`here `. +Now that you have properly set up your vision system and have tuned a pipeline, you can now aim your robot at an AprilTag using the data from PhotonVision. The *yaw* of the target is the critical piece of data that will be needed first. -For this simple example, only yaw is needed. +Yaw is reported to the roboRIO over Network Tables. PhotonLib, our vender dependency, is the easiest way to access this data. The documentation for the Network Tables API can be found {ref}`here ` and the documentation for PhotonLib {ref}`here `. -In this example, while the operator holds a button down, the robot will turn towards the goal using the P term of a PID loop. To learn more about how PID loops work, how WPILib implements them, and more, visit [Advanced Controls (PID)](https://docs.wpilib.org/en/stable/docs/software/advanced-control/introduction/index.html) and [PID Control in WPILib](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/pidcontroller.html#pid-control-in-wpilib). +In this example, while the operator holds a button down, the robot will turn towards the AprilTag using the P term of a PID loop. To learn more about how PID loops work, how WPILib implements them, and more, visit [Advanced Controls (PID)](https://docs.wpilib.org/en/stable/docs/software/advanced-control/introduction/index.html) and [PID Control in WPILib](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/pidcontroller.html#pid-control-in-wpilib). ```{eval-rst} .. tab-set:: .. tab-item:: Java - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java :language: java - :lines: 41-98 + :lines: 77-117 :linenos: - :lineno-start: 41 + :lineno-start: 77 .. tab-item:: C++ (Header) - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h :language: c++ - :lines: 27-53 + :lines: 25-60 :linenos: - :lineno-start: 27 + :lineno-start: 25 .. tab-item:: C++ (Source) - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp :language: c++ - :lines: 25-52 + :lines: 56-96 :linenos: - :lineno-start: 25 + :lineno-start: 56 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-python-examples/aimattarget/robot.py + :language: python + :lines: 46-70 + :linenos: + :lineno-start: 46 + ``` diff --git a/docs/source/docs/examples/gettinginrangeofthetarget.md b/docs/source/docs/examples/gettinginrangeofthetarget.md deleted file mode 100644 index c53b01c9b..000000000 --- a/docs/source/docs/examples/gettinginrangeofthetarget.md +++ /dev/null @@ -1,58 +0,0 @@ -# Getting in Range of the Target - -The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/master/photonlib-java-examples/getinrange)/[C++](https://github.com/PhotonVision/photonvision/tree/master/photonlib-cpp-examples/getinrange)). - -## Knowledge and Equipment Needed - -- Everything required in {ref}`Aiming at a Target `. -- Large space where your robot can move around freely - -## Code - -In FRC, a mechanism usually has to be a certain distance away from its target in order to be effective and score. In the previous example, we showed how to aim your robot at the target. Now we will show how to move to a certain distance from the target. - -For proper functionality of just this example, ensure that your robot is pointed towards the target. - -While the operator holds down a button, the robot will drive towards the target and get in range. - -This example uses P term of the PID loop and PhotonLib and the distance function of PhotonUtils. - -:::{warning} -The PhotonLib utility to calculate distance depends on the camera being at a different vertical height than the target. If this is not the case, a different method for estimating distance, such as target width or area, should be used. In general, this method becomes more accurate as range decreases and as the height difference increases. -::: - -:::{note} -There is no strict minimum delta-height necessary for this method to be applicable, just a requirement that a delta exists. -::: - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/getinrange/src/main/java/frc/robot/Robot.java - :language: java - :lines: 42-107 - :linenos: - :lineno-start: 42 - - .. tab-item:: C++ (Header) - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-cpp-examples/getinrange/src/main/include/Robot.h - :language: c++ - :lines: 27-67 - :linenos: - :lineno-start: 27 - - .. tab-item:: C++ (Source) - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp - :language: c++ - :lines: 25-58 - :linenos: - :lineno-start: 25 -``` - -:::{hint} -The accuracy of the measurement of the camera's pitch ({code}`CAMERA_PITCH_RADIANS` in the above example), as well as the camera's FOV, will determine the overall accuracy of this method. -::: diff --git a/docs/source/docs/examples/images/poseest_demo.gif b/docs/source/docs/examples/images/poseest_demo.gif new file mode 100644 index 000000000..532672537 Binary files /dev/null and b/docs/source/docs/examples/images/poseest_demo.gif differ diff --git a/docs/source/docs/examples/index.md b/docs/source/docs/examples/index.md index 1737b68e5..501b1b535 100644 --- a/docs/source/docs/examples/index.md +++ b/docs/source/docs/examples/index.md @@ -4,8 +4,6 @@ :maxdepth: 1 aimingatatarget -gettinginrangeofthetarget aimandrange -simaimandrange -simposeest +poseest ``` diff --git a/docs/source/docs/examples/poseest.md b/docs/source/docs/examples/poseest.md new file mode 100644 index 000000000..3d10e6c87 --- /dev/null +++ b/docs/source/docs/examples/poseest.md @@ -0,0 +1,211 @@ +# Using WPILib Pose Estimation, Simulation, and PhotonVision Together + +The following example comes from the PhotonLib example repository ([Java](https://github.com/gerth2/photonvision/tree/master/photonlib-java-examples/poseest)/[C++](https://github.com/gerth2/photonvision/tree/master/photonlib-cpp-examples/poseest)/[Python](https://github.com/gerth2/photonvision/tree/master/photonlib-python-examples/poseest)). Full code is available at that links. + +## Knowledge and Equipment Needed + +- Everything required in {ref}`Combining Aiming and Getting in Range `, plus some familiarity with WPILib pose estimation functionality. + +## Background + +This example demonstrates integration of swerve drive control, a basic swerve physics simulation, and PhotonLib's simulated vision system functionality. + +## Walkthrough + +### Estimating Pose + +The {code}`Drivetrain` class includes functionality to fuse multiple sensor readings together (including PhotonVision) into a best-guess of the pose on the field. + +Please reference the [WPILib documentation](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-pose_state-estimators.html) on using the {code}`SwerveDrivePoseEstimator` class. + +We use the 2024 game's AprilTag Locations: + +```{eval-rst} +.. tab-set:: + + .. tab-item:: Java + :sync: java + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 68-68 + :linenos: + :lineno-start: 68 + + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Constants.h + :language: c++ + :lines: 42-43 + :linenos: + :lineno-start: 42 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 46-46 + :linenos: + :lineno-start: 46 + +``` + + + +To incorporate PhotonVision, we need to create a {code}`PhotonCamera`: + + +```{eval-rst} +.. tab-set:: + + .. tab-item:: Java + :sync: java + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 57-57 + :linenos: + :lineno-start: 57 + + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 145-145 + :linenos: + :lineno-start: 145 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 44-44 + :linenos: + :lineno-start: 44 +``` + +During periodic execution, we read back camera results. If we see AprilTags in the image, we calculate the camera-measured pose of the robot and pass it to the {code}`Drivetrain`. + +```{eval-rst} +.. tab-set:: + + .. tab-item:: Java + :sync: java + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java + :language: java + :lines: 64-74 + :linenos: + :lineno-start: 64 + + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp + :language: c++ + :lines: 38-46 + :linenos: + :lineno-start: 38 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 54-56 + :linenos: + :lineno-start: 54 + +``` + +### Simulating the Camera + +First, we create a new {code}`VisionSystemSim` to represent our camera and coprocessor running PhotonVision, and moving around our simulated field. + +```{eval-rst} +.. tab-set:: + + .. tab-item:: Java + :sync: java + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 65-69 + :linenos: + :lineno-start: 65 + + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 49-52 + :linenos: + :lineno-start: 49 + + .. tab-item:: Python + + # Coming Soon! + +``` + +Then, we add configure the simulated vision system to match the camera system being simulated. + +```{eval-rst} +.. tab-set:: + + .. tab-item:: Java + :sync: java + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 69-82 + :linenos: + :lineno-start: 69 + + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 53-65 + :linenos: + :lineno-start: 53 + + .. tab-item:: Python + + # Coming Soon! +``` + + +### Updating the Simulated Vision System + +During simulation, we periodically update the simulated vision system. + +```{eval-rst} +.. tab-set:: + + .. tab-item:: Java + :sync: java + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java + :language: java + :lines: 114-132 + :linenos: + :lineno-start: 114 + + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp + :language: c++ + :lines: 95-109 + :linenos: + :lineno-start: 95 + + .. tab-item:: Python + + # Coming Soon! +``` + +The rest is done behind the scenes. + +```{image} images/poseest_demo.gif +:alt: Simulated swerve drive and vision system working together in teleoperated mode. +:width: 1200 +``` diff --git a/docs/source/docs/examples/simaimandrange.md b/docs/source/docs/examples/simaimandrange.md deleted file mode 100644 index 403c8d537..000000000 --- a/docs/source/docs/examples/simaimandrange.md +++ /dev/null @@ -1,96 +0,0 @@ -# Simulating Aiming and Getting in Range - -The following example comes from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/661f8b2c0495474015f6ea9a89d65f9788436a05/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange)/[C++](https://github.com/PhotonVision/photonvision/tree/661f8b2c0495474015f6ea9a89d65f9788436a05/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange)). Full code is available at those links. - -## Knowledge and Equipment Needed - -- Everything required in {ref}`Combining Aiming and Getting in Range `. - -## Background - -The previous examples show how to run PhotonVision on a real robot, with a physical robot drivetrain moving around and interacting with the software. - -This example builds upon that, adding support for simulating robot motion and incorporating that motion into a {code}`SimVisionSystem`. This allows you to test control algorithms on your development computer, without requiring access to a real robot. - -```{raw} html - -``` - -## Walkthrough - -First, in the main {code}`Robot` source file, we add support to periodically update a new simulation-specific object. This logic only gets used while running in simulation: - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java - :language: java - :lines: 118-128 - :linenos: - :lineno-start: 118 -``` - -Then, we add in the implementation of our new `DrivetrainSim` class. Please reference the [WPILib documentation on physics simulation](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/physics-sim.html). - -Simulated Vision support is added with the following steps: - -### Creating the Simulated Vision System - -First, we create a new {code}`SimVisionSystem` to represent our camera and coprocessor running PhotonVision. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 73-93 - :linenos: - :lineno-start: 72 -``` - -Next, we create objects to represent the physical location and size of the vision targets we are calibrated to detect. This example models the down-field high goal vision target from the 2020 and 2021 games. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 95-111 - :linenos: - :lineno-start: 95 -``` - -Finally, we add our target to the simulated vision system. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 116-117 - :linenos: - :lineno-start: 113 - -``` - -If you have additional targets you want to detect, you can add them in the same way as the first one. - -### Updating the Simulated Vision System - -Once we have all the properties of our simulated vision system defined, the work to do at runtime becomes very minimal. Simply pass in the robot's pose periodically to the simulated vision system. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/ebef19af3d926cf87292177c9a16d01b71219306/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 124-142 - :linenos: - :lineno-start: 122 - -``` - -The rest is done behind the scenes. diff --git a/docs/source/docs/examples/simposeest.md b/docs/source/docs/examples/simposeest.md deleted file mode 100644 index 9e9f6913f..000000000 --- a/docs/source/docs/examples/simposeest.md +++ /dev/null @@ -1,133 +0,0 @@ -# Using WPILib Pose Estimation, Simulation, and PhotonVision Together - -The following example comes from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/master/photonlib-java-examples/)). Full code is available at that links. - -## Knowledge and Equipment Needed - -- Everything required in {ref}`Combining Aiming and Getting in Range `, plus some familiarity with WPILib pose estimation functionality. - -## Background - -This example builds upon WPILib's [Differential Drive Pose Estimator](https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator). It adds a {code}`PhotonCamera` to gather estimates of the robot's position on the field. This in turn can be used for aligning with vision targets, and increasing accuracy of autonomous routines. - -To support simulation, a {code}`SimVisionSystem` is used to drive data into the {code}`PhotonCamera`. The far high goal target from 2020 is modeled. - -## Walkthrough - -WPILib's {code}`Pose2d` class is used to represent robot positions on the field. - -Three different {code}`Pose2d` positions are relevant for this example: - -1. Desired Pose: The location some autonomous routine wants the robot to be in. -2. Estimated Pose: The location the software `believes` the robot to be in, based on physics models and sensor feedback. -3. Actual Pose: The locations the robot is actually at. The physics simulation generates this in simulation, but it cannot be directly measured on the real robot. - -### Estimating Pose - -The {code}`DrivetrainPoseEstimator` class is responsible for generating an estimated robot pose using sensor readings (including PhotonVision). - -Please reference the [WPILib documentation](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-pose_state-estimators.html) on using the {code}`DifferentialDrivePoseEstimator` class. - -For both simulation and on-robot code, we create objects to represent the physical location and size of the vision targets we are calibrated to detect. This example models the down-field high goal vision target from the 2020 and 2021 games. - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - :sync: java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java - :language: java - :lines: 83-106 - :linenos: - :lineno-start: 83 - -``` - -To incorporate PhotonVision, we need to create a {code}`PhotonCamera`: - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - :sync: java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java - :language: java - :lines: 46 - :linenos: - :lineno-start: 46 -``` - -During periodic execution, we read back camera results. If we see a target in the image, we pass the camera-measured pose of the robot to the {code}`DifferentialDrivePoseEstimator`. - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - :sync: java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java - :language: java - :lines: 81-92 - :linenos: - :lineno-start: 81 - -``` - -That's it! - -### Simulating the Camera - -First, we create a new {code}`SimVisionSystem` to represent our camera and coprocessor running PhotonVision. - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - :sync: java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java - :language: java - :lines: 76-95 - :linenos: - :lineno-start: 76 - -``` - -Then, we add our target to the simulated vision system. - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - :sync: java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java - :lines: 97-99 - :linenos: - :lineno-start: 97 - -``` - -If you have additional targets you want to detect, you can add them in the same way as the first one. - -### Updating the Simulated Vision System - -Once we have all the properties of our simulated vision system defined, the remaining work is minimal. Periodically, pass in the robot's pose to the simulated vision system. - -```{eval-rst} -.. tab-set:: - - .. tab-item:: Java - :sync: java - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java - :language: java - :lines: 138-139 - :linenos: - :lineno-start: 138 - -``` - -The rest is done behind the scenes. diff --git a/docs/source/docs/installation/networking.md b/docs/source/docs/installation/networking.md index fd46ad387..efe8762c1 100644 --- a/docs/source/docs/installation/networking.md +++ b/docs/source/docs/installation/networking.md @@ -44,13 +44,17 @@ If you would like to access your Ethernet-connected vision device from a compute ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java PortForwarder.add(5800, "photonvision.local", 5800); .. code-block:: C++ wpi::PortForwarder::GetInstance().Add(5800, "photonvision.local", 5800); + + .. code-block:: Python + + # Coming Soon! ``` :::{note} diff --git a/docs/source/docs/installation/sw_install/other-coprocessors.md b/docs/source/docs/installation/sw_install/other-coprocessors.md index d6b6fc277..c2e0c7ff2 100644 --- a/docs/source/docs/installation/sw_install/other-coprocessors.md +++ b/docs/source/docs/installation/sw_install/other-coprocessors.md @@ -1,7 +1,7 @@ # Other Debian-Based Co-Processor Installation :::{warning} -Working with unsupported coprocessors requires some level of "know how" of your target system. The install script has only been tested on Debian/Raspberry Pi OS Buster and Ubuntu Bionic. If any issues arise with your specific OS, please open an issue on our [issues page](https://github.com/PhotonVision/photonvision/issues). +Working with unsupported coprocessors requires some level of "know how" of your system. The install script has only been tested on Debian/Raspberry Pi OS Buster and Ubuntu Bionic. If any issues arise with your specific OS, please open an issue on our [issues page](https://github.com/PhotonVision/photonvision/issues). ::: :::{note} diff --git a/docs/source/docs/integration/advancedStrategies.md b/docs/source/docs/integration/advancedStrategies.md index 4d02547b1..54f66f124 100644 --- a/docs/source/docs/integration/advancedStrategies.md +++ b/docs/source/docs/integration/advancedStrategies.md @@ -1,8 +1,12 @@ # Advanced Strategies -Advanced strategies for using vision processing results involve working with the robot's *pose* on the field. A *pose* is a combination an X/Y coordinate, and an angle describing where the robot's front is pointed. It is always considered *relative* to some fixed point on the field. +Advanced strategies for using vision processing results involve working with the robot's *pose* on the field. -WPILib provides a [Pose2d](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html) class to describe poses in software. +A *pose* is a combination an X/Y coordinate, and an angle describing where the robot's front is pointed. A pose is always considered *relative* to some fixed point on the field. + +WPILib provides a [Pose2d](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html) class to describe poses in software. + +PhotonVision can supply correcting information to keep estimates of *pose* accurate over a full match. ## Knowledge and Equipment Needed @@ -12,30 +16,53 @@ WPILib provides a [Pose2d](https://docs.wpilib.org/en/stable/docs/software/advan \- Sufficient sensors to measure wheel rotation \- Capable of closed-loop velocity control - A gyroscope or IMU measuring actual robot heading -- Experience using some path-planning library (WPILib is our recommendation) +- Experience using some path-planning library -## Path Planning in a Target-Centered Reference Frame +## Robot Poses from the Camera -When using 3D mode in PhotonVision, the [SolvePNP Algorithm](https://en.wikipedia.org/wiki/Perspective-n-Point) is used to deduce the *camera's* position in a 3D coordinate system centered on the target itself. +When using 3D mode in PhotonVision, an additional step is run to estimate the 3D position of camera, relative to one or more AprilTags. -A simple algorithm for using this measurement is: +This process does not produce a *unique* solution. There are multiple possible camera positions which might explain the image it observed. Additionally, the camera is rarely mounted in the exact center of a robot. -1. Assume your robot needs to be at a fixed `Pose2D` *relative to the target*. -2. When triggered: - #. Read the most recent vision measurement - this is your *actual* pose. - #. Generate a simple trajectory to the goal position - #. Execute the trajectory +For these reasons, the 3D information must be filtered and transformed before they can describe the robot's pose. -:::{note} -There is not currently an example demonstrating this technique. -::: +PhotonLib provides {ref}`a utility class to assist with this process on the roboRIO `. Alternatively, {ref}`a "multi-tag" strategy can do this process on the coprocessor. `. -## Global Pose Estimation +## Field-Relative Pose Estimation -A more complex way to utilize a camera-supplied `Pose2D` is to incorporate it into an estimation of the robot's `Pose2D` in a global field reference frame. +The camera's guess of the robot pose generally should be *fused* with other sensor readings. -When using this strategy, the measurements made by the camera are *fused* with measurements from other sensors, a model of expected robot behavior, and a matrix of weights that describes how trustworthy each sensor is. The result is a *best-guess* at the current pose on the field. +WPILib provides [a set of pose estimation classes](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) for doing this work. -In turn, this best-guess position is used to path plan to the known positions on the field, which may or may not have vision targets nearby. +## I have a Pose Estimate, Now What? -See the {ref}`Pose Estimation ` example for more information. +### Triggering Actions Automatically + +A simple way to use a pose estimate is to activate robot functions automatically when in the correct spot on the field. + +```{eval-rst} +.. tab-set-code:: + + .. code-block:: Java + + Pose3d robotPose; + boolean launcherSpinCmd; + + // ... + + if(robotPose.X() < 1.5){ + // Near blue alliance wall, start spinning the launcher wheel + launcherSpinCmd = True; + } else { + // Far away, no need to run launcher. + launcherSpinCmd = False; + } + + // ... +``` + +### PathPlanning + +A common, but more complex usage of a pose estimate is an input to a path-following algorithm. Specifically, the pose estimate is used to correct for the robot straying off of the pre-defined path. + +See the {ref}`Pose Estimation ` example for details on integrating this. diff --git a/docs/source/docs/integration/aprilTagStrategies.md b/docs/source/docs/integration/aprilTagStrategies.md deleted file mode 100644 index 3e181a765..000000000 --- a/docs/source/docs/integration/aprilTagStrategies.md +++ /dev/null @@ -1,46 +0,0 @@ -# AprilTag Strategies - -:::{note} -The same strategies covered in the simple and advanced strategy sections still apply to AprilTags, and we encourage you to read them first. This page will discuss the specific nuances to using AprilTags. -::: - -## Simple Strategies - -Prior to the introduction of AprilTags, the most common vision strategy for teams was to use the yaw of the detected target in order to turn to the target, and then score. This is still possible with AprilTags as the yaw of the tag is reported. Similarly, getting the distance to the target via trigonometry will also work. This is discussed in greater detail in the previous page. - -## Advanced Strategies - -AprilTags allows you find the robot pose on the field using data from the tags. A pose is a combination an X/Y coordinate, and an angle describing where the robot’s front is pointed. It is always considered relative to some fixed point on the field. - -### Knowledge and Equipment Needed - -Knowledge - -- How to tune an AprilTag Pipeline (found in the pipeline tuning section) - -Equipment - -- A Coprocessor running PhotonVision - Accurate camera calibration to support “3D mode” required -- A Drivetrain with wheels and sensors (Sufficient sensors to measure wheel rotation and capable of closed-loop velocity control) -- A gyroscope or IMU measuring actual robot heading - -### Global Pose Estimation / Pose Estimation Strategies - -:::{note} -See the previous page for more general information. Most of the information is the same except now the camera is supplying a `Pose3D`. -::: - -The nature of how AprilTags will be laid out makes it very likely that you will get multiple pose measurements within a single frame from seeing multiple targets. This requires strategies to fuse these observations together and get a "best guess" as to where your robot is. The best way to do this is to use the corners from all visible AprilTags to estimate the robot's pose. This is done by using the `PhotonPoseEstimator` class and the "MULTI_TAG_PNP_ON_COPROCESSOR" strategy. Additional strategies include: - -- A camera seeing multiple targets, taking the average of all the returned poses -- A camera seeing one target, with an assumed height off the ground, picking the pose which places it to the assumed height -- A camera seeing one target, and picking a pose most similar to the most recently observed pose -- A camera seeing one target, and picking a pose most similar to one provided externally (ie, from previous loop's odometry) -- A camera seeing one target, and picking the pose with the lowest ambiguity. - -PhotonVision supports all of these different strategies via our `PhotonPoseEstimator` class that allows you to select one of the strategies above and get the relevant pose estimation. - -### Tuning Pose Estimators - -Coming soon! -TODO: Add this back in once simposeest example is added. diff --git a/docs/source/docs/integration/index.md b/docs/source/docs/integration/index.md index e38af90b6..b5bc24339 100644 --- a/docs/source/docs/integration/index.md +++ b/docs/source/docs/integration/index.md @@ -6,5 +6,4 @@ background simpleStrategies advancedStrategies -aprilTagStrategies ``` diff --git a/docs/source/docs/integration/simpleStrategies.md b/docs/source/docs/integration/simpleStrategies.md index 5edf49878..1e90b5c52 100644 --- a/docs/source/docs/integration/simpleStrategies.md +++ b/docs/source/docs/integration/simpleStrategies.md @@ -1,36 +1,32 @@ # Simple Strategies -Simple strategies for using vision processor outputs involve using the target's position in the 2D image to infer *range* and *angle* to the target. +Simple strategies for using vision processor outputs involve using the target's position in the 2D image to infer *range* and *angle* to a particular AprilTag. ## Knowledge and Equipment Needed - A Coprocessor running PhotonVision - A Drivetrain with wheels +- An AprilTag to aim at ## Angle Alignment -The simplest way to use a vision processing result is to first determine how far left or right in the image the vision target should be for your robot to be "aligned" to the target. Then, +The simplest way to align a robot to an AprilTag is to rotate the drivetrain until the tag is centered in the camera image. To do this, -1. Read the current angle to the target from the vision Coprocessor. -2. If too far in one direction, command the drivetrain to rotate in the opposite direction to compensate. +1. Read the current yaw angle to the AprilTag from the vision Coprocessor. +2. If too far off to one side, command the drivetrain to rotate in the opposite direction to compensate. See the {ref}`Aiming at a Target ` example for more information. -:::{note} -Sometimes, these strategies have also involved incorporating a gyroscope. This can be necessary due to the high latency of vision processing algorithms. However, advancements in the tools available (including PhotonVision) has made that unnecessary for most applications. -::: +NOTE: This works if the camera is centered on the robot. This is easiest from a software perspective. If the camera is not centered, take a peek at the next example - it shows how to account for an offset. -## Range Alignment +## Adding Range Alignment -By looking at the position of the target in the "vertical" direction in the image, and applying some trigonometry, the distance between the robot and the camera can be deduced. +By looking at the position of the AprilTag in the "vertical" direction in the image, and applying some trigonometry, the distance between the robot and the camera can be deduced. -1. Read the current distance to the target from the vision coprocessor. +1. Read the current pitch angle to the AprilTag from the vision coprocessor. +2. Do math to calculate the distance to the AprilTag. 2. If too far in one direction, command the drivetrain to travel in the opposite direction to compensate. -See the {ref}`Getting in Range of the Target ` example for more information. - -## Angle + Range - -Since the previous two alignment strategies work on independent axes of the robot, there's no reason you can't do them simultaneously. +This can be done simultaneously while aligning to the desired angle. See the {ref}`Aim and Range ` example for more information. diff --git a/docs/source/docs/programming/photonlib/controlling-led.md b/docs/source/docs/programming/photonlib/controlling-led.md index 34649ddfd..7bd6fd0c3 100644 --- a/docs/source/docs/programming/photonlib/controlling-led.md +++ b/docs/source/docs/programming/photonlib/controlling-led.md @@ -4,13 +4,17 @@ You can control the vision LEDs of supported hardware via PhotonLib using the `s ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Blink the LEDs. camera.setLED(VisionLEDMode.kBlink); - .. code-block:: c++ + .. code-block:: C++ // Blink the LEDs. camera.SetLED(photonlib::VisionLEDMode::kBlink); + + .. code-block:: Python + + # Coming Soon! ``` diff --git a/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md b/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md index 2b9444dc2..9a531dde9 100644 --- a/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md +++ b/docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md @@ -9,7 +9,7 @@ You can use the `setDriverMode()`/`SetDriverMode()` (Java and C++ respectively) ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Set driver mode to on. camera.setDriverMode(true); @@ -18,6 +18,10 @@ You can use the `setDriverMode()`/`SetDriverMode()` (Java and C++ respectively) // Set driver mode to on. camera.SetDriverMode(true); + + .. code-block:: Python + + # Coming Soon! ``` ## Setting the Pipeline Index @@ -27,7 +31,7 @@ You can use the `setPipelineIndex()`/`SetPipelineIndex()` (Java and C++ respecti ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Change pipeline to 2 camera.setPipelineIndex(2); @@ -36,6 +40,10 @@ You can use the `setPipelineIndex()`/`SetPipelineIndex()` (Java and C++ respecti // Change pipeline to 2 camera.SetPipelineIndex(2); + + .. code-block:: Python + + # Coming Soon! ``` ## Getting the Pipeline Latency @@ -44,15 +52,19 @@ You can also get the pipeline latency from a pipeline result using the `getLaten ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Get the pipeline latency. double latencySeconds = result.getLatencyMillis() / 1000.0; - .. code-block:: c++ + .. code-block:: C++ // Get the pipeline latency. units::second_t latency = result.GetLatency(); + + .. code-block:: Python + + # Coming Soon! ``` :::{note} diff --git a/docs/source/docs/programming/photonlib/getting-target-data.md b/docs/source/docs/programming/photonlib/getting-target-data.md index f22e83755..1d14088cf 100644 --- a/docs/source/docs/programming/photonlib/getting-target-data.md +++ b/docs/source/docs/programming/photonlib/getting-target-data.md @@ -20,7 +20,7 @@ The `PhotonCamera` class has two constructors: one that takes a `NetworkTable` a :language: c++ :lines: 42-43 - .. code-block:: python + .. code-block:: Python # Change this to match the name of your camera as shown in the web ui self.camera = PhotonCamera("your_camera_name_here") @@ -51,7 +51,7 @@ Use the `getLatestResult()`/`GetLatestResult()` (Java and C++ respectively) to o :language: c++ :lines: 35-36 - .. code-block:: python + .. code-block:: Python # Query the latest result from PhotonVision result = self.camera.getLatestResult() @@ -69,17 +69,17 @@ Each pipeline result has a `hasTargets()`/`HasTargets()` (Java and C++ respectiv ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Check if the latest result has any targets. boolean hasTargets = result.hasTargets(); - .. code-block:: c++ + .. code-block:: C++ // Check if the latest result has any targets. bool hasTargets = result.HasTargets(); - .. code-block:: python + .. code-block:: Python # Check if the latest result has any targets. hasTargets = result.hasTargets() @@ -99,17 +99,17 @@ You can get a list of tracked targets using the `getTargets()`/`GetTargets()` (J ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Get a list of currently tracked targets. List targets = result.getTargets(); - .. code-block:: c++ + .. code-block:: C++ // Get a list of currently tracked targets. wpi::ArrayRef targets = result.GetTargets(); - .. code-block:: python + .. code-block:: Python # Get a list of currently tracked targets. targets = result.getTargets() @@ -121,20 +121,20 @@ You can get the {ref}`best target corners = target.getCorners(); - .. code-block:: c++ + .. code-block:: C++ // Get information from target. double yaw = target.GetYaw(); @@ -169,7 +169,7 @@ You can get the {ref}`best target , 4> corners = target.GetCorners(); - .. code-block:: python + .. code-block:: Python # Get information from target. yaw = target.getYaw() @@ -193,7 +193,7 @@ All of the data above (**except skew**) is available when using AprilTags. ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Get information from target. int targetID = target.getFiducialId(); @@ -201,7 +201,7 @@ All of the data above (**except skew**) is available when using AprilTags. Transform3d bestCameraToTarget = target.getBestCameraToTarget(); Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - .. code-block:: c++ + .. code-block:: C++ // Get information from target. int targetID = target.GetFiducialId(); @@ -209,7 +209,7 @@ All of the data above (**except skew**) is available when using AprilTags. frc::Transform3d bestCameraToTarget = target.getBestCameraToTarget(); frc::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - .. code-block:: python + .. code-block:: Python # Get information from target. targetID = target.getFiducialId() @@ -227,7 +227,7 @@ Images are stored within the PhotonVision configuration directory. Running the " ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Capture pre-process camera stream image camera.takeInputSnapshot(); @@ -243,7 +243,7 @@ Images are stored within the PhotonVision configuration directory. Running the " // Capture post-process camera stream image camera.TakeOutputSnapshot(); - .. code-block:: python + .. code-block:: Python # Capture pre-process camera stream image camera.takeInputSnapshot() diff --git a/docs/source/docs/programming/photonlib/robot-pose-estimator.md b/docs/source/docs/programming/photonlib/robot-pose-estimator.md index 2e288f3fc..283d86bb5 100644 --- a/docs/source/docs/programming/photonlib/robot-pose-estimator.md +++ b/docs/source/docs/programming/photonlib/robot-pose-estimator.md @@ -14,16 +14,20 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // The field from AprilTagFields will be different depending on the game. AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - .. code-block:: c++ + .. code-block:: C++ // The parameter for LoadAPrilTagLayoutField will be different depending on the game. frc::AprilTagFieldLayout aprilTagFieldLayout = frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo); + .. code-block:: Python + + # Coming Soon! + ``` ## Creating a `PhotonPoseEstimator` @@ -46,7 +50,7 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java //Forward Camera cam = new PhotonCamera("testCamera"); @@ -55,7 +59,7 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s // Construct PhotonPoseEstimator PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cam, robotToCam); - .. code-block:: c++ + .. code-block:: C++ // Forward Camera std::shared_ptr cameraOne = @@ -76,6 +80,22 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s photonlib::RobotPoseEstimator estimator( aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras); + + .. code-block:: Python + + kRobotToCam = wpimath.geometry.Transform3d( + wpimath.geometry.Translation3d(0.5, 0.0, 0.5), + wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0), + ) + + self.cam = PhotonCamera("YOUR CAMERA NAME") + + self.camPoseEst = PhotonPoseEstimator( + loadAprilTagLayoutField(AprilTagField.k2024Crescendo), + PoseStrategy.CLOSEST_TO_REFERENCE_POSE, + self.cam, + kRobotToCam + ) ``` ## Using a `PhotonPoseEstimator` @@ -88,7 +108,7 @@ Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotP :language: java :lines: 85-88 - .. code-block:: c++ + .. code-block:: C++ std::pair getEstimatedGlobalPose( frc::Pose3d prevEstimatedRobotPose) { @@ -102,6 +122,11 @@ Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotP return std::make_pair(frc::Pose2d(), 0_ms); } } + + .. code-block:: Python + + + ``` You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `RobotPoseEstimator` every loop using `addVisionMeasurement()`. TODO: add example note diff --git a/docs/source/docs/programming/photonlib/using-target-data.md b/docs/source/docs/programming/photonlib/using-target-data.md index c5f67ce8a..ad4c516cf 100644 --- a/docs/source/docs/programming/photonlib/using-target-data.md +++ b/docs/source/docs/programming/photonlib/using-target-data.md @@ -1,6 +1,6 @@ # Using Target Data -A `PhotonUtils` class with helpful common calculations is included within `PhotonLib` to aid teams in using target data in order to get positional information on the field. This class contains two methods, `calculateDistanceToTargetMeters()`/`CalculateDistanceToTarget()` and `estimateTargetTranslation2d()`/`EstimateTargetTranslation()` (Java and C++ respectively). +A `PhotonUtils` class with helpful common calculations is included within `PhotonLib` to aid teams in using AprilTag data in order to get positional information on the field. This class contains two methods, `calculateDistanceToTargetMeters()`/`CalculateDistanceToTarget()` and `estimateTargetTranslation2d()`/`EstimateTargetTranslation()` (Java and C++ respectively). ## Estimating Field Relative Pose with AprilTags @@ -8,13 +8,17 @@ A `PhotonUtils` class with helpful common calculations is included within `Photo ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Calculate robot's field relative pose Pose3d robotPose = PhotonUtils.estimateFieldToRobotAprilTag(target.getBestCameraToTarget(), aprilTagFieldLayout.getTagPose(target.getFiducialId()), cameraToRobot); - .. code-block:: c++ + .. code-block:: C++ //TODO + + .. code-block:: Python + + # Coming Soon! ``` ## Estimating Field Relative Pose (Traditional) @@ -23,18 +27,22 @@ You can get your robot's `Pose2D` on the field using various camera data, target ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Calculate robot's field relative pose Pose2D robotPose = PhotonUtils.estimateFieldToRobot( kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, Rotation2d.fromDegrees(-target.getYaw()), gyro.getRotation2d(), targetPose, cameraToRobot); - .. code-block:: c++ + .. code-block:: C++ // Calculate robot's field relative pose frc::Pose2D robotPose = photonlib::EstimateFieldToRobot( kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, frc::Rotation2d(units::degree_t(-target.GetYaw())), frc::Rotation2d(units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot); + .. code-block:: Python + + # Coming Soon! + ``` ## Calculating Distance to Target @@ -44,14 +52,18 @@ If your camera is at a fixed height on your robot and the height of the target i ```{eval-rst} .. tab-set-code:: + .. code-block:: Java - .. rli:: https://github.com/PhotonVision/photonvision/raw/a3bcd3ac4f88acd4665371abc3073bdbe5effea8/photonlib-java-examples/src/main/java/org/photonlib/examples/getinrange/Robot.java - :language: java - :lines: 78-94 + // TODO + + .. code-block:: C++ + + // TODO + + .. code-block:: Python + + # Coming Soon! - .. rli:: https://github.com/PhotonVision/photonvision/raw/a3bcd3ac4f88acd4665371abc3073bdbe5effea8/photonlib-cpp-examples/src/main/cpp/examples/getinrange/cpp/Robot.cpp - :language: cpp - :lines: 33-46 ``` :::{note} @@ -64,13 +76,17 @@ The C++ version of PhotonLib uses the Units library. For more information, see [ ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java double distanceToTarget = PhotonUtils.getDistanceToPose(robotPose, targetPose); - .. code-block:: c++ + .. code-block:: C++ //TODO + + .. code-block:: Python + + # Coming Soon! ``` ## Estimating Camera Translation to Target @@ -79,17 +95,22 @@ You can get a [translation](https://docs.wpilib.org/en/latest/docs/software/adva ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Calculate a translation from the camera to the target. Translation2d translation = PhotonUtils.estimateCameraToTargetTranslation( distanceMeters, Rotation2d.fromDegrees(-target.getYaw())); - .. code-block:: c++ + .. code-block:: C++ // Calculate a translation from the camera to the target. frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslationn( distance, frc::Rotation2d(units::degree_t(-target.GetYaw()))); + + .. code-block:: Python + + # Coming Soon! + ``` :::{note} @@ -102,10 +123,14 @@ We are negating the yaw from the camera from CV (computer vision) conventions to ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java Rotation2d targetYaw = PhotonUtils.getYawToPose(robotPose, targetPose); - .. code-block:: c++ + .. code-block:: C++ //TODO + + .. code-block:: Python + + # Coming Soon! ``` diff --git a/docs/source/docs/reflectiveAndShape/3D.md b/docs/source/docs/reflectiveAndShape/3D.md index 043c82ff8..db0a745a4 100644 --- a/docs/source/docs/reflectiveAndShape/3D.md +++ b/docs/source/docs/reflectiveAndShape/3D.md @@ -1,6 +1,6 @@ # 3D Tuning -In 3D mode, the SolvePNP algorithm is used to compute the position and rotation of the target relative to the robot. This requires your {ref}`camera to be calibrated ` which can be done through the cameras tab. +In 3D mode, the SolvePNP algorithm is used to compute the position and rotation of the AprilTag or other target relative to the robot. This requires your {ref}`camera to be calibrated ` which can be done through the cameras tab. The target model dropdown is used to select the target model used to compute target position. This should match the target your camera will be tracking. @@ -17,6 +17,6 @@ If solvePNP is working correctly, the target should be displayed as a small rect ``` -## Contour Simplification +## Contour Simplification (Non-Apriltag) 3D mode internally computes a polygon that approximates the target contour being tracked. This polygon is used to detect the extreme corners of the target. The contour simplification slider changes how far from the original contour the approximation is allowed to deviate. Note that the approximate polygon is drawn on the output image for tuning. diff --git a/docs/source/docs/simulation/hardware-in-the-loop-sim.md b/docs/source/docs/simulation/hardware-in-the-loop-sim.md index b23276b1d..5897d0b91 100644 --- a/docs/source/docs/simulation/hardware-in-the-loop-sim.md +++ b/docs/source/docs/simulation/hardware-in-the-loop-sim.md @@ -2,7 +2,7 @@ Hardware in the loop simulation is using a physical device, such as a supported co-processor running PhotonVision, to enhance simulation capabilities. This is useful for developing and validating code before the camera is attached to a robot, as well as reducing the work required to use WPILib simulation with PhotonVision. -Before continuing, ensure PhotonVision is installed on your target device. Instructions can be found {ref}`here ` for all devices. +Before continuing, ensure PhotonVision is installed on your device. Instructions can be found {ref}`here ` for all devices. Your coprocessor and computer running simulation will have to be connected to the same network, like a home router. Connecting the coprocessor directly to the computer will not work. diff --git a/docs/source/docs/simulation/index.md b/docs/source/docs/simulation/index.md index a63a143a9..a4f007cfe 100644 --- a/docs/source/docs/simulation/index.md +++ b/docs/source/docs/simulation/index.md @@ -4,7 +4,8 @@ :maxdepth: 0 :titlesonly: true -simulation -simulation-deprecated +simulation-java +simulation-cpp +simulation-python hardware-in-the-loop-sim ``` diff --git a/docs/source/docs/simulation/simulation-cpp.md b/docs/source/docs/simulation/simulation-cpp.md new file mode 100644 index 000000000..ccb28de0b --- /dev/null +++ b/docs/source/docs/simulation/simulation-cpp.md @@ -0,0 +1,5 @@ +# Simulation Support in PhotonLib in C++ + +## What Is Supported? + +Nothing yet. diff --git a/docs/source/docs/simulation/simulation-deprecated.md b/docs/source/docs/simulation/simulation-deprecated.md deleted file mode 100644 index ba7be7e45..000000000 --- a/docs/source/docs/simulation/simulation-deprecated.md +++ /dev/null @@ -1,103 +0,0 @@ -# Simulation Support in PhotonLib (Deprecated) - -:::{attention} -This page details the pre-2024 simulation support. For current Java simulation support, see {doc}`/docs/simulation/simulation`. -::: - -## What Is Supported? - -PhotonLib supports simulation of a camera and coprocessor running PhotonVision moving about a field on a robot. - -You can use this to help validate your robot code's behavior in simulation without needing a physical robot. - -## Simulation Vision World Model - -Sim-specific classes are provided to model sending one frame of a camera image through PhotonVision. Based on what targets are visible, results are published to NetworkTables. - -While processing, the given robot `Pose2d` is used to analyze which targets should be in view, and determine where they would have shown up in the camera image. - -Targets are considered in view if: - -1. Their centroid is within the field of view of the camera. -2. The camera is not in driver mode. -3. The target's in-image pixel size is greater than `minTargetArea` -4. The distance from the camera to the target is less than `maxLEDRange` - -:::{warning} -Not all network tables objects are updated in simulation. The interaction through PhotonLib remains the same. Actual camera images are also not simulated. -::: - -Latency of processing is not yet modeled. - -```{image} diagrams/SimArchitecture-deprecated.drawio.svg -:alt: A diagram comparing the architecture of a real PhotonVision process to a simulated -: one. -``` - -## Simulated Vision System - -A `SimVisionSystem` represents the camera and coprocessor running PhotonVision moving around on the field. - -It requires a number of pieces of configuration to accurately simulate your physical setup. Match them to your configuration in PhotonVision, and to your robot's physical dimensions. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 73-93 -``` - -After declaring the system, you should create and add one `SimVisionTarget` per target you are attempting to detect. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 95-111 -``` - -Finally, while running the simulation, process simulated camera frames by providing the robot's pose to the system. - -```{eval-rst} -.. tab-set-code:: - - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/80e16ece87c735e30755dea271a56a2ce217b588/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java - :language: java - :lines: 138-139 -``` - -This will cause most NetworkTables fields to update properly, representing any targets that are in view of the robot. - -Robot software which uses PhotonLib to interact with a camera running PhotonVision should work the same as though a real camera was hooked up and active. - -## Raw-Data Approach - -Users may wish to directly provide target information based on an existing detailed simulation. - -A `SimPhotonCamera` can be created for this purpose. It provides an interface where the user can supply target data via a list of `PhotonTrackedTarget` objects. - -```{eval-rst} -.. tab-set-code:: - - .. code-block:: java - - @Override - public void simulationInit() { - // ... - cam = new SimPhotonCamera("MyCamera"); - // ... - } - - @Override - public void simulationPeriodic() { - // ... - ArrayList visibleTgtList = new ArrayList(); - visibleTgtList.add(new PhotonTrackedTarget(yawDegrees, pitchDegrees, area, skew, camToTargetTrans)); // Repeat for each target that you see - cam.submitProcessedFrame(0.0, visibleTgtList); - // ... - } -``` - -Note that while there is less code and configuration required to get basic data into the simulation, this approach will cause the user to need to implement much more code on their end to calculate the relative positions of the robot and target. If you already have this, the raw interface may be helpful. However, if you don't, you'll likely want to be looking at the Simulated Vision System first. diff --git a/docs/source/docs/simulation/simulation.md b/docs/source/docs/simulation/simulation-java.md similarity index 89% rename from docs/source/docs/simulation/simulation.md rename to docs/source/docs/simulation/simulation-java.md index 2c23708d1..7b4f99da9 100644 --- a/docs/source/docs/simulation/simulation.md +++ b/docs/source/docs/simulation/simulation-java.md @@ -1,14 +1,11 @@ -# Simulation Support in PhotonLib +# Simulation Support in PhotonLib in Java -:::{attention} -This page details the current simulation support for Java. For other languages, see {doc}`/docs/simulation/simulation-deprecated`. -::: ## What Is Simulated? Simulation is a powerful tool for validating robot code without access to a physical robot. Read more about [simulation in WPILib](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-simulation/introduction.html). -PhotonLib can simulate cameras on the field and generate target data approximating what would be seen in reality. This simulation attempts to include the following: +In Java, PhotonLib can simulate cameras on the field and generate target data approximating what would be seen in reality. This simulation attempts to include the following: - Camera Properties - Field of Vision @@ -58,7 +55,7 @@ A `VisionSystemSim` represents the simulated world for one or more cameras, and ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // A vision system sim labelled as "main" in NetworkTables VisionSystemSim visionSim = new VisionSystemSim("main"); @@ -71,7 +68,7 @@ Vision targets require a `TargetModel`, which describes the shape of the target. ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // A 0.5 x 0.25 meter rectangular target TargetModel targetModel = new TargetModel(0.5, 0.25); @@ -82,7 +79,7 @@ These `TargetModel` are paired with a target pose to create a `VisionTargetSim`. ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // The pose of where the target is on the field. // Its rotation determines where "forward" or the target x-axis points. @@ -104,7 +101,7 @@ For convenience, an `AprilTagFieldLayout` can also be added to automatically cre ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // The layout of AprilTags which we want to add to the vision system AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); @@ -125,7 +122,7 @@ Before adding a simulated camera, we need to define its properties. This is done ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // The simulated camera properties SimCameraProperties cameraProp = new SimCameraProperties(); @@ -136,7 +133,7 @@ By default, this will create a 960 x 720 resolution camera with a 90 degree diag ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // A 640 x 480 camera with a 100 degree diagonal FOV. cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); @@ -154,7 +151,7 @@ These properties are used in a `PhotonCameraSim`, which handles generating captu ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // The PhotonCamera used in the real robot code. PhotonCamera camera = new PhotonCamera("cameraName"); @@ -168,7 +165,7 @@ The `PhotonCameraSim` can now be added to the `VisionSystemSim`. We have to defi ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Our camera is mounted 0.1 meters forward and 0.5 meters up from the robot pose, // (Robot pose is considered the center of rotation at the floor level, or Z = 0) @@ -190,7 +187,7 @@ If the camera is mounted on a mobile mechanism (like a turret) this transform ca ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // The turret the camera is mounted on is rotated 5 degrees Rotation3d turretRotation = new Rotation3d(0, 0, Math.toRadians(5)); @@ -207,7 +204,7 @@ To update the `VisionSystemSim`, we simply have to pass in the simulated robot p ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Update with the simulated drivetrain pose. This should be called every loop in simulation. visionSim.update(robotPoseMeters); @@ -222,14 +219,14 @@ Each `VisionSystemSim` has its own built-in `Field2d` for displaying object pose ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Get the built-in Field2d used by this VisionSystemSim visionSim.getDebugField(); ``` :::{figure} images/SimExampleField.png -*A* `VisionSystemSim`*'s internal* `Field2d` *customized with target images and colors, as seen in the* [swervedriveposeestsim](https://github.com/PhotonVision/photonvision/tree/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim) *example.* +*A* `VisionSystemSim`*'s internal* `Field2d` *customized with target images and colors* ::: A `PhotonCameraSim` can also draw and publish generated camera frames to a MJPEG stream similar to an actual PhotonVision process. @@ -237,7 +234,7 @@ A `PhotonCameraSim` can also draw and publish generated camera frames to a MJPEG ```{eval-rst} .. tab-set-code:: - .. code-block:: java + .. code-block:: Java // Enable the raw and processed streams. These are enabled by default. cameraSim.enableRawStream(true); @@ -251,5 +248,5 @@ A `PhotonCameraSim` can also draw and publish generated camera frames to a MJPEG These streams follow the port order mentioned in {ref}`docs/installation/networking:Camera Stream Ports`. For example, a single simulated camera will have its raw stream at `localhost:1181` and processed stream at `localhost:1182`, which can also be found in the CameraServer tab of Shuffleboard like a normal camera stream. :::{figure} images/SimExampleFrame.png -*A frame from the processed stream of a simulated camera viewing some 2023 AprilTags with the field wireframe enabled, as seen in the* [swervedriveposeestsim example](https://github.com/PhotonVision/photonvision/tree/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim). +*A frame from the processed stream of a simulated camera viewing some 2023 AprilTags with the field wireframe enabled* ::: diff --git a/docs/source/docs/simulation/simulation-python.md b/docs/source/docs/simulation/simulation-python.md new file mode 100644 index 000000000..eb871c6bb --- /dev/null +++ b/docs/source/docs/simulation/simulation-python.md @@ -0,0 +1,5 @@ +# Simulation Support in PhotonLib in Python + +## What Is Supported? + +Nothing Yet diff --git a/docs/source/index.md b/docs/source/index.md index a49e19131..f624ad155 100644 --- a/docs/source/index.md +++ b/docs/source/index.md @@ -45,7 +45,7 @@ Welcome to the official documentation of PhotonVision! PhotonVision is the free, :link: docs/hardware/index :link-type: doc - Select appropriate hardware for high-quality and easy vision target detection. + Select appropriate hardware for high-quality and easy vision target detection. .. grid-item-card:: Contributing :link: docs/contributing/index diff --git a/photon-lib/py/disableUsingDevBuilds.bat b/photon-lib/py/disableUsingDevBuilds.bat new file mode 100644 index 000000000..bff0ccdcd --- /dev/null +++ b/photon-lib/py/disableUsingDevBuilds.bat @@ -0,0 +1,4 @@ +@echo off +setlocal + +python -m pip config unset global.find-links diff --git a/photon-lib/py/enableUsingDevBuilds.bat b/photon-lib/py/enableUsingDevBuilds.bat new file mode 100644 index 000000000..645fa0a40 --- /dev/null +++ b/photon-lib/py/enableUsingDevBuilds.bat @@ -0,0 +1,4 @@ +@echo off +setlocal + +python -m pip config set global.find-links %~dp0\dist 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 @@ from ..targeting import * 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 @@ from ..targeting import * 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 @@ from ..targeting import * 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 @@ from ..targeting import * 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 @@ from ..targeting import * 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 @@ from ..targeting import * 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 @@ from photonlibpy.photonCamera import setVersionCheckEnabled 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-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index 9ae88e157..c3ee9cbee 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -11,6 +11,10 @@ repositories { jcenter() } +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.3.2" +wpi.versions.wpimathVersion = "2024.3.2" + apply from: "${rootDir}/../shared/examples_common.gradle" // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/photonlib-cpp-examples/apriltagExample/networktables.json b/photonlib-cpp-examples/aimandrange/networktables.json similarity index 100% rename from photonlib-cpp-examples/apriltagExample/networktables.json rename to photonlib-cpp-examples/aimandrange/networktables.json diff --git a/photonlib-cpp-examples/aimandrange/settings.gradle b/photonlib-cpp-examples/aimandrange/settings.gradle index b9552a3f3..44fbca751 100644 --- a/photonlib-cpp-examples/aimandrange/settings.gradle +++ b/photonlib-cpp-examples/aimandrange/settings.gradle @@ -1,6 +1,6 @@ import org.gradle.internal.os.OperatingSystem -rootProject.name = 'aimandrange' +rootProject.name = 'aimattarget' pluginManagement { repositories { diff --git a/photonlib-cpp-examples/aimandrange/simgui-ds.json b/photonlib-cpp-examples/aimandrange/simgui-ds.json index bd35d5052..addd5860c 100644 --- a/photonlib-cpp-examples/aimandrange/simgui-ds.json +++ b/photonlib-cpp-examples/aimandrange/simgui-ds.json @@ -11,10 +11,13 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], "axisCount": 6, diff --git a/photonlib-cpp-examples/aimandrange/simgui-window.json b/photonlib-cpp-examples/aimandrange/simgui-window.json index bf1709620..3baff34d0 100644 --- a/photonlib-cpp-examples/aimandrange/simgui-window.json +++ b/photonlib-cpp-examples/aimandrange/simgui-window.json @@ -1,50 +1,64 @@ { + "Docking": { + "Data": [] + }, "MainWindow": { "GLOBAL": { - "height": "720", + "fps": "120", + "height": "910", "maximized": "0", "style": "0", "userScale": "2", - "width": "1733", - "xpos": "3306", - "ypos": "476" + "width": "1550", + "xpos": "-1602", + "ypos": "79" } }, "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "554,10", + "Size": "991,527" + }, "###FMS": { "Collapsed": "0", - "Pos": "5,540", - "Size": "283,146" + "Pos": "12,604", + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", - "Pos": "255,463", - "Size": "796,155" - }, - "###Keyboard 0 Settings": { - "Collapsed": "0", - "Pos": "10,50", - "Size": "300,560" + "Pos": "278,685", + "Size": "976,219" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "367,58", - "Size": "750,386" + "Pos": "353,479", + "Size": "830,620" + }, + "###NetworkTables Info": { + "Collapsed": "0", + "Pos": "315,527", + "Size": "750,145" }, "###Other Devices": { "Collapsed": "0", "Pos": "1025,20", "Size": "250,695" }, + "###Plot <0>": { + "Collapsed": "0", + "Pos": "113,22", + "Size": "448,400" + }, "###System Joysticks": { "Collapsed": "0", - "Pos": "15,306", - "Size": "192,218" + "Pos": "5,350", + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,127" + "Size": "162,142" }, "Debug##Default": { "Collapsed": "0", @@ -54,7 +68,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "109,134" } } } diff --git a/photonlib-cpp-examples/aimandrange/simgui.json b/photonlib-cpp-examples/aimandrange/simgui.json index 94b13e3bd..3f4675ea3 100644 --- a/photonlib-cpp-examples/aimandrange/simgui.json +++ b/photonlib-cpp-examples/aimandrange/simgui.json @@ -1,16 +1,163 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { - "photonvision": { - "open": true, - "photonvision": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, + "SmartDashboard": { + "VisionSystemSim-main": { "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)" + } + ] } + ], + "window": { + "visible": false } } } diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp index ac98dcd49..5bd1690fb 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp @@ -26,44 +26,106 @@ #include +#include + +#include +#include + +void Robot::RobotInit() {} + +void Robot::RobotPeriodic() { + drivetrain.Periodic(); + drivetrain.Log(); +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() {} + +void Robot::AutonomousPeriodic() {} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() { + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); +} + void Robot::TeleopPeriodic() { - double forwardSpeed; - double rotationSpeed; - - if (xboxController.GetAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - const auto& result = camera.GetLatestResult(); + // Calculate drivetrain commands from Joystick values + auto forward = + -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed; + auto strafe = + -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed; + auto turn = + -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed; + bool targetVisible = false; + units::degree_t targetYaw = 0.0_deg; + units::meter_t targetRange = 0.0_m; + auto results = camera.GetAllUnreadResults(); + if (results.size() > 0) { + // Camera processed a new frame since last + // Get the last one in the list. + auto result = results[results.size() - 1]; if (result.HasTargets()) { - // First calculate range - units::meter_t range = photon::PhotonUtils::CalculateDistanceToTarget( - CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, - units::degree_t{result.GetBestTarget().GetPitch()}); - - // Use this range as the measurement we give to the PID controller. - // -1.0 required to ensure positive PID controller effort _increases_ - // range - forwardSpeed = -forwardController.Calculate(range.value(), - GOAL_RANGE_METERS.value()); - - // Also calculate angular power - // -1.0 required to ensure positive PID controller effort _increases_ yaw - rotationSpeed = - -turnController.Calculate(result.GetBestTarget().GetYaw(), 0); - } else { - // If we have no targets, stay still. - forwardSpeed = 0; - rotationSpeed = 0; + // At least one AprilTag was seen by the camera + for (auto& target : result.GetTargets()) { + if (target.GetFiducialId() == 7) { + // Found Tag 7, record its information + targetYaw = units::degree_t{target.GetYaw()}; + targetRange = photon::PhotonUtils::CalculateDistanceToTarget( + 0.5_m, // Measured with a tape measure, or in CAD + 1.435_m, // From 2024 game manual for ID 7 + -30.0_deg, // Measured witha protractor, or in CAD + units::degree_t{target.GetPitch()}); + targetVisible = true; + } + } } - } else { - // Manual Driver Mode - forwardSpeed = -xboxController.GetRightY(); - rotationSpeed = xboxController.GetLeftX(); } - // Use our forward/turn speeds to control the drivetrain - drive.ArcadeDrive(forwardSpeed, rotationSpeed); + // Auto-align when requested + if (controller.GetAButton() && 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 and gets the range right. + turn = (VISION_DES_ANGLE - targetYaw).value() * VISION_TURN_kP * + constants::Swerve::kMaxAngularSpeed; + forward = (VISION_DES_RANGE - targetRange).value() * VISION_STRAFE_kP * + constants::Swerve::kMaxLinearSpeed; + } + + // Command drivetrain motors based on target speeds + drivetrain.Drive(forward, strafe, turn); +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 000000000..92801c3e4 --- /dev/null +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,200 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDrive.h" + +#include +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, true, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); + } + gyroSim.SetAngle(-pose.Rotation().Degrees()); + gyroSim.SetRate(0_rad_per_s); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp rename to photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp rename to photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/aimandrange/src/main/deploy/example.txt similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt rename to photonlib-cpp-examples/aimandrange/src/main/deploy/example.txt diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h b/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h new file mode 100644 index 000000000..6a20d6799 --- /dev/null +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +inline const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, -30_deg, 0_rad}}; +inline const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)}; + +inline const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +inline constexpr units::meter_t kTrackWidth{18.5_in}; +inline constexpr units::meter_t kTrackLength{18.5_in}; +inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr units::meter_t kWheelDiameter{4_in}; +inline constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +inline constexpr double kDriveGearRatio = 6.75; +inline constexpr double kSteerGearRatio = 12.8; + +inline constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +inline constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +inline constexpr double kDriveKP = 1.0; +inline constexpr double kDriveKI = 0.0; +inline constexpr double kDriveKD = 0.0; + +inline constexpr double kSteerKP = 20.0; +inline constexpr double kSteerKI = 0.0; +inline constexpr double kSteerKD = 0.25; + +inline const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +inline const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +inline const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +inline const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +inline const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +inline const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h index c7483a417..f68262385 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h @@ -28,44 +28,36 @@ #include #include -#include -#include -#include -#include -#include + +#include "Constants.h" +#include "VisionSim.h" +#include "subsystems/SwerveDrive.h" class Robot : public frc::TimedRobot { public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; private: - // Constants such as camera and target height stored. Change per robot and - // goal! - const units::meter_t CAMERA_HEIGHT = 24_in; - const units::meter_t TARGET_HEIGHT = 5_ft; - - // Angle between horizontal and the camera. - const units::radian_t CAMERA_PITCH = 0_deg; - - // How far from the target we want to be - const units::meter_t GOAL_RANGE_METERS = 3_ft; - - // PID constants should be tuned per robot - const double LINEAR_P = 0.1; - const double LINEAR_D = 0.0; - frc::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D}; - - const double ANGULAR_P = 0.1; - const double ANGULAR_D = 0.0; - frc::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D}; - - // Change this to match the name of your camera - photon::PhotonCamera camera{"photonvision"}; - - frc::XboxController xboxController{0}; - - // Drive motors - frc::PWMVictorSPX leftMotor{0}; - frc::PWMVictorSPX rightMotor{1}; - frc::DifferentialDrive drive{leftMotor, rightMotor}; + photon::PhotonCamera camera{constants::Vision::kCameraName}; + SwerveDrive drivetrain{}; + VisionSim vision{&camera}; + frc::XboxController controller{0}; + static constexpr auto VISION_TURN_kP = 0.01; + static constexpr auto VISION_DES_ANGLE = 0.0_deg; + static constexpr auto VISION_STRAFE_kP = 0.5; + static constexpr auto VISION_DES_RANGE = 1.25_m; }; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h b/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h new file mode 100644 index 000000000..9fe64fb5c --- /dev/null +++ b/photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h @@ -0,0 +1,87 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class VisionSim { + public: + explicit VisionSim(photon::PhotonCamera* camera) { + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(70_Hz); + cameraProp->SetAvgLatency(30_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = + std::make_shared(camera, *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; } + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + + // The most recent result, cached for calculating std devs + photon::PhotonPipelineResult m_latestResult; +}; diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 000000000..98b0b2a58 --- /dev/null +++ b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,80 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h rename to photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h rename to photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/aimandrange/swerve_module.png similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png rename to photonlib-cpp-examples/aimandrange/swerve_module.png diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/aimandrange/tag-blue.png similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png rename to photonlib-cpp-examples/aimandrange/tag-blue.png diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/aimandrange/tag-green.png similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/tag-green.png rename to photonlib-cpp-examples/aimandrange/tag-green.png diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index 1fe7ace22..c3ee9cbee 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -11,18 +11,12 @@ repositories { jcenter() } +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.3.2" +wpi.versions.wpimathVersion = "2024.3.2" + apply from: "${rootDir}/../shared/examples_common.gradle" -ext { - wpilibVersion = "2025.0.0-alpha-1" - wpimathVersion = wpilibVersion - openCVversion = "4.8.0-2" -} - -wpi.getVersions().getOpencvVersion().convention(openCVversion); -wpi.getVersions().getWpilibVersion().convention(wpilibVersion); -wpi.getVersions().getWpimathVersion().convention(wpimathVersion); - // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { @@ -68,21 +62,36 @@ wpi.sim.addDriverstation() model { components { frcUserProgram(NativeExecutableSpec) { - targetPlatform wpi.platforms.roborio - targetPlatform wpi.platforms.desktop + // We don't need to build for roborio -- if we do, we need to install + // a roborio toolchain every time we build in CI + // Ideally, we'd be able to set the roborio toolchain as optional, but + // I can't figure out how to set that environment variable from build.gradle + // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) + // for now, commented out + + // targetPlatform wpi.platforms.roborio + + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } sources.cpp { source { srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' } exportedHeaders { srcDir 'src/main/include' } } + // Set deploy task to deploy this component deployArtifact.component = it + + // Enable run tasks for this component wpi.cpp.enableExternalTasks(it) + // Enable simulation for this component wpi.sim.enable(it) // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. wpi.cpp.vendor.cpp(it) @@ -100,6 +109,7 @@ model { } } + // Enable run tasks for this component wpi.cpp.enableExternalTasks(it) wpi.cpp.vendor.cpp(it) diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/aimattarget/networktables.json similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/networktables.json rename to photonlib-cpp-examples/aimattarget/networktables.json diff --git a/photonlib-cpp-examples/aimattarget/simgui-ds.json b/photonlib-cpp-examples/aimattarget/simgui-ds.json index bd35d5052..addd5860c 100644 --- a/photonlib-cpp-examples/aimattarget/simgui-ds.json +++ b/photonlib-cpp-examples/aimattarget/simgui-ds.json @@ -11,10 +11,13 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], "axisCount": 6, diff --git a/photonlib-cpp-examples/aimattarget/simgui-window.json b/photonlib-cpp-examples/aimattarget/simgui-window.json index 1d727b721..1a49507f8 100644 --- a/photonlib-cpp-examples/aimattarget/simgui-window.json +++ b/photonlib-cpp-examples/aimattarget/simgui-window.json @@ -1,50 +1,64 @@ { + "Docking": { + "Data": [] + }, "MainWindow": { "GLOBAL": { - "height": "720", + "fps": "120", + "height": "910", "maximized": "0", "style": "0", "userScale": "2", - "width": "1280", - "xpos": "3124", - "ypos": "324" + "width": "1550", + "xpos": "-1602", + "ypos": "79" } }, "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "554,10", + "Size": "991,527" + }, "###FMS": { "Collapsed": "0", - "Pos": "5,540", - "Size": "283,146" + "Pos": "12,604", + "Size": "336,158" }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,465", - "Size": "796,155" - }, - "###Keyboard 0 Settings": { - "Collapsed": "0", - "Pos": "10,50", - "Size": "300,560" + "Pos": "278,685", + "Size": "976,219" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "250,277", - "Size": "750,185" + "Pos": "353,479", + "Size": "830,620" + }, + "###NetworkTables Info": { + "Collapsed": "0", + "Pos": "315,527", + "Size": "750,145" }, "###Other Devices": { "Collapsed": "0", "Pos": "1025,20", "Size": "250,695" }, + "###Plot <0>": { + "Collapsed": "0", + "Pos": "113,22", + "Size": "448,400" + }, "###System Joysticks": { "Collapsed": "0", "Pos": "5,350", - "Size": "192,218" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,127" + "Size": "162,142" }, "Debug##Default": { "Collapsed": "0", @@ -54,7 +68,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "109,134" } } } diff --git a/photonlib-cpp-examples/aimattarget/simgui.json b/photonlib-cpp-examples/aimattarget/simgui.json index 449f4b1d6..3f4675ea3 100644 --- a/photonlib-cpp-examples/aimattarget/simgui.json +++ b/photonlib-cpp-examples/aimattarget/simgui.json @@ -1,7 +1,164 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, + "SmartDashboard": { + "VisionSystemSim-main": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)" + } + ] + } + ], + "window": { + "visible": false + } } } } diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index 830fe147c..140af4563 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -24,50 +24,98 @@ #include "Robot.h" -#include +#include -#include -#include -#include +#include +#include + +void Robot::RobotInit() {} void Robot::RobotPeriodic() { - photon::PhotonCamera::SetVersionCheckEnabled(false); + drivetrain.Periodic(); + drivetrain.Log(); +} - auto start = frc::Timer::GetFPGATimestamp(); - photon::PhotonPipelineResult result = camera.GetLatestResult(); - auto end = frc::Timer::GetFPGATimestamp(); +void Robot::DisabledInit() {} - std::printf("DT is %.2f uS for %i targets\n", - units::microsecond_t(end - start).to(), - result.GetTargets().size()); +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() {} + +void Robot::AutonomousPeriodic() {} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() { + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); } void Robot::TeleopPeriodic() { - double forwardSpeed = -xboxController.GetRightY(); - double rotationSpeed; - - if (xboxController.GetAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - auto start = frc::Timer::GetFPGATimestamp(); - photon::PhotonPipelineResult result = camera.GetLatestResult(); - auto end = frc::Timer::GetFPGATimestamp(); - frc::SmartDashboard::PutNumber("decode_dt", (end - start).to()); + // Calculate drivetrain commands from Joystick values + auto forward = + -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed; + auto strafe = + -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed; + auto turn = + -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed; + bool targetVisible = false; + double targetYaw = 0.0; + auto results = camera.GetAllUnreadResults(); + if (results.size() > 0) { + // Camera processed a new frame since last + // Get the last one in the list. + auto result = results[results.size() - 1]; if (result.HasTargets()) { - // Rotation speed is the output of the PID controller - rotationSpeed = -controller.Calculate(result.GetBestTarget().GetYaw(), 0); - } else { - // If we have no targets, stay still. - rotationSpeed = 0; + // At least one AprilTag was seen by the camera + for (auto& target : result.GetTargets()) { + if (target.GetFiducialId() == 7) { + // Found Tag 7, record its information + targetYaw = target.GetYaw(); + targetVisible = true; + } + } } - } else { - // Manual Driver Mode - rotationSpeed = xboxController.GetLeftX(); } - // Use our forward/turn speeds to control the drivetrain - drive.ArcadeDrive(forwardSpeed, rotationSpeed); + // Auto-align when requested + if (controller.GetAButton() && 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. + turn = + -1.0 * targetYaw * VISION_TURN_kP * constants::Swerve::kMaxAngularSpeed; + } + + // Command drivetrain motors based on target speeds + drivetrain.Drive(forward, strafe, turn); +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 000000000..92801c3e4 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,200 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDrive.h" + +#include +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, true, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); + } + gyroSim.SetAngle(-pose.Rotation().Degrees()); + gyroSim.SetRate(0_rad_per_s); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 000000000..31bbe09e6 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 000000000..785d9ca7d --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,147 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveModule.h" + +#include +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt b/photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt new file mode 100644 index 000000000..15bc5c1eb --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h new file mode 100644 index 000000000..6a20d6799 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +inline constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +inline const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, -30_deg, 0_rad}}; +inline const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)}; + +inline const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +inline const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +inline constexpr units::meter_t kTrackWidth{18.5_in}; +inline constexpr units::meter_t kTrackLength{18.5_in}; +inline constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +inline constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +inline constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +inline constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +inline constexpr units::meter_t kWheelDiameter{4_in}; +inline constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +inline constexpr double kDriveGearRatio = 6.75; +inline constexpr double kSteerGearRatio = 12.8; + +inline constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +inline constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +inline constexpr double kDriveKP = 1.0; +inline constexpr double kDriveKI = 0.0; +inline constexpr double kDriveKD = 0.0; + +inline constexpr double kSteerKP = 20.0; +inline constexpr double kSteerKI = 0.0; +inline constexpr double kSteerKD = 0.25; + +inline const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +inline const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +inline const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +inline const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +inline const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +inline const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h index aae3a98c6..2c06b7ae6 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h @@ -28,27 +28,33 @@ #include #include -#include -#include -#include -#include -#include + +#include "Constants.h" +#include "VisionSim.h" +#include "subsystems/SwerveDrive.h" class Robot : public frc::TimedRobot { public: - void TeleopPeriodic() override; + void RobotInit() override; void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; private: - // Change this to match the name of your camera as shown in the web UI - photon::PhotonCamera camera{"WPI2024"}; - // PID constants should be tuned per robot - frc::PIDController controller{.1, 0, 0}; - - frc::XboxController xboxController{0}; - - // Drive motors - frc::PWMVictorSPX leftMotor{0}; - frc::PWMVictorSPX rightMotor{1}; - frc::DifferentialDrive drive{leftMotor, rightMotor}; + photon::PhotonCamera camera{constants::Vision::kCameraName}; + SwerveDrive drivetrain{}; + VisionSim vision{&camera}; + frc::XboxController controller{0}; + static constexpr double VISION_TURN_kP = 0.01; }; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h b/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h new file mode 100644 index 000000000..9fe64fb5c --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h @@ -0,0 +1,87 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class VisionSim { + public: + explicit VisionSim(photon::PhotonCamera* camera) { + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(320, 240, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(70_Hz); + cameraProp->SetAvgLatency(30_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = + std::make_shared(camera, *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; } + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + + // The most recent result, cached for calculating std devs + photon::PhotonPipelineResult m_latestResult; +}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 000000000..98b0b2a58 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,80 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 000000000..c1cee3e6f --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 000000000..444e4bdf4 --- /dev/null +++ b/photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photonlib-java-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/aimattarget/swerve_module.png similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/swerve_module.png rename to photonlib-cpp-examples/aimattarget/swerve_module.png diff --git a/photonlib-java-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/aimattarget/tag-blue.png similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/tag-blue.png rename to photonlib-cpp-examples/aimattarget/tag-blue.png diff --git a/photonlib-java-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/aimattarget/tag-green.png similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/tag-green.png rename to photonlib-cpp-examples/aimattarget/tag-green.png diff --git a/photonlib-cpp-examples/apriltagExample/build.gradle b/photonlib-cpp-examples/apriltagExample/build.gradle deleted file mode 100644 index 9ae88e157..000000000 --- a/photonlib-cpp-examples/apriltagExample/build.gradle +++ /dev/null @@ -1,116 +0,0 @@ -plugins { - id "cpp" - id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.3.2" - - id "com.dorongold.task-tree" version "2.1.0" -} - -repositories { - mavenLocal() - jcenter() -} - -apply from: "${rootDir}/../shared/examples_common.gradle" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcCpp - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Set to true to run simulation in debug mode -wpi.cpp.debugSimulation = false - -// Default enable simgui -wpi.sim.addGui().defaultEnabled = true -// Enable DS but not by default -wpi.sim.addDriverstation() - -model { - components { - frcUserProgram(NativeExecutableSpec) { - // We don't need to build for roborio -- if we do, we need to install - // a roborio toolchain every time we build in CI - // Ideally, we'd be able to set the roborio toolchain as optional, but - // I can't figure out how to set that environment variable from build.gradle - // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) - // for now, commented out - - // targetPlatform wpi.platforms.roborio - - if (includeDesktopSupport) { - targetPlatform wpi.platforms.desktop - } - - sources.cpp { - source { - srcDir 'src/main/cpp' - include '**/*.cpp', '**/*.cc' - } - exportedHeaders { - srcDir 'src/main/include' - } - } - - // Set deploy task to deploy this component - deployArtifact.component = it - - // Enable run tasks for this component - wpi.cpp.enableExternalTasks(it) - - // Enable simulation for this component - wpi.sim.enable(it) - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - } - } - testSuites { - frcUserProgramTest(GoogleTestTestSuiteSpec) { - testing $.components.frcUserProgram - - sources.cpp { - source { - srcDir 'src/test/cpp' - include '**/*.cpp' - } - } - - // Enable run tasks for this component - wpi.cpp.enableExternalTasks(it) - - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - wpi.cpp.deps.googleTest(it) - } - } -} diff --git a/photonlib-cpp-examples/apriltagExample/simgui-window.json b/photonlib-cpp-examples/apriltagExample/simgui-window.json deleted file mode 100644 index 79d676b16..000000000 --- a/photonlib-cpp-examples/apriltagExample/simgui-window.json +++ /dev/null @@ -1,81 +0,0 @@ -{ - "Docking": { - "Data": [] - }, - "MainWindow": { - "GLOBAL": { - "fps": "120", - "height": "880", - "maximized": "0", - "style": "0", - "userScale": "2", - "width": "1652", - "xpos": "268", - "ypos": "82" - } - }, - "Table": { - "0x542B5671,2": { - "Column 0 Width": "391", - "Column 1 Width": "156", - "RefScale": "13" - } - }, - "Window": { - "###/SmartDashboard/Field": { - "Collapsed": "0", - "Pos": "514,2", - "Size": "517,341" - }, - "###FMS": { - "Collapsed": "0", - "Pos": "36,663", - "Size": "283,146" - }, - "###Joysticks": { - "Collapsed": "0", - "Pos": "373,500", - "Size": "796,206" - }, - "###Keyboard 0 Settings": { - "Collapsed": "0", - "Pos": "149,98", - "Size": "300,560" - }, - "###NetworkTables": { - "Collapsed": "0", - "Pos": "663,464", - "Size": "750,365" - }, - "###NetworkTables Info": { - "Collapsed": "0", - "Pos": "520,330", - "Size": "750,145" - }, - "###Other Devices": { - "Collapsed": "0", - "Pos": "1025,20", - "Size": "250,695" - }, - "###System Joysticks": { - "Collapsed": "0", - "Pos": "5,350", - "Size": "192,218" - }, - "###Timing": { - "Collapsed": "0", - "Pos": "5,150", - "Size": "135,127" - }, - "Debug##Default": { - "Collapsed": "0", - "Pos": "60,60", - "Size": "400,400" - }, - "Robot State": { - "Collapsed": "0", - "Pos": "5,20", - "Size": "92,99" - } - } -} diff --git a/photonlib-cpp-examples/apriltagExample/simgui.json b/photonlib-cpp-examples/apriltagExample/simgui.json deleted file mode 100644 index bb09beb6e..000000000 --- a/photonlib-cpp-examples/apriltagExample/simgui.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" - }, - "windows": { - "/SmartDashboard/Field": { - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "transitory": { - "CameraPublisher": { - "open": true - }, - "SmartDashboard": { - "open": true - }, - "photonvision": { - "open": true, - "photonvision": { - "open": true - } - } - } - }, - "NetworkTables Info": { - "Clients": { - "open": true - }, - "Server": { - "open": true - }, - "visible": true - } -} diff --git a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp deleted file mode 100644 index c92980b7f..000000000 --- a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "Drivetrain.h" - -#include -#include - -void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { - auto leftFeedforward = m_feedforward.Calculate(speeds.left); - auto rightFeedforward = m_feedforward.Calculate(speeds.right); - double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(), - speeds.left.value()); - double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(), - speeds.right.value()); - - m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); - m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); -} - -void Drivetrain::Drive(units::meters_per_second_t xSpeed, - units::radians_per_second_t rot) { - SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); -} - -void Drivetrain::UpdateOdometry() { - m_poseEstimator.Update(m_gyro.GetRotation2d(), - units::meter_t{m_leftEncoder.GetDistance()}, - units::meter_t{m_rightEncoder.GetDistance()}); -} - -void Drivetrain::ResetOdometry(const frc::Pose2d& pose) { - m_leftEncoder.Reset(); - m_rightEncoder.Reset(); - m_drivetrainSimulator.SetPose(pose); - m_poseEstimator.ResetPosition( - m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()}, - units::meter_t{m_rightEncoder.GetDistance()}, pose); -} - -void Drivetrain::SimulationPeriodic() { - // To update our simulation, we set motor voltage inputs, update the - // simulation, and write the simulated positions and velocities to our - // simulated encoder and gyro. We negate the right side so that positive - // voltages make the right side move forward. - - m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} * - frc::RobotController::GetInputVoltage(), - units::volt_t{m_rightGroup.Get()} * - frc::RobotController::GetInputVoltage()); - m_drivetrainSimulator.Update(20_ms); - - m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value()); - m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value()); - m_rightEncoderSim.SetDistance( - m_drivetrainSimulator.GetRightPosition().value()); - m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value()); - m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value()); -} - -void Drivetrain::Periodic() { - UpdateOdometry(); - - auto result = m_pcw.Update(m_poseEstimator.GetEstimatedPosition()); - if (result) { - m_poseEstimator.AddVisionMeasurement( - result.value().estimatedPose.ToPose2d(), result.value().timestamp); - } - - m_fieldSim.SetRobotPose(m_poseEstimator.GetEstimatedPosition()); -} diff --git a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp deleted file mode 100644 index 8a84b64f4..000000000 --- a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "Robot.h" - -#include -#include - -void Robot::RobotInit() { - if constexpr (frc::RobotBase::IsSimulation()) { - auto inst = nt::NetworkTableInstance::GetDefault(); - inst.StopServer(); - // set the NT server if simulating this code. - // "localhost" for photon on desktop, or "photonvision.local" or - // "[ip-address]" for coprocessor - inst.SetServer("localhost"); - inst.StartClient4("Robot Simulation"); - } -} - -void Robot::TeleopPeriodic() { - // Get the x speed. We are inverting this because Xbox controllers return - // negative values when we push forward. - const auto xSpeed = -m_controller.GetLeftY() * Drivetrain::kMaxSpeed; - - // Get the rate of angular rotation. We are inverting this because we want a - // positive value when we pull to the left (remember, CCW is positive in - // mathematics). Xbox controllers return positive values when you pull to - // the right by default. - auto rot = -m_controller.GetRightX() * Drivetrain::kMaxAngularSpeed; - - m_drive.Drive(xSpeed, rot); -} - -void Robot::RobotPeriodic() { m_drive.Periodic(); } -void Robot::SimulationPeriodic() { m_drive.SimulationPeriodic(); } - -#ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } -#endif diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h b/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h deleted file mode 100644 index 903256cbe..000000000 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h +++ /dev/null @@ -1,140 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "PhotonCameraWrapper.h" - -/** - * Represents a differential drive style drivetrain. - */ -class Drivetrain { - public: - Drivetrain() { - m_gyro.Reset(); - - // We need to invert one side of the drivetrain so that positive voltages - // result in both sides moving forward. Depending on how your robot's - // gearbox is constructed, you might have to invert the left side instead. - m_rightGroup.SetInverted(true); - - // Set the distance per pulse for the drive encoders. We can simply use the - // distance traveled for one rotation of the wheel divided by the encoder - // resolution. - m_leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius / - kEncoderResolution); - m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius / - kEncoderResolution); - - m_leftEncoder.Reset(); - m_rightEncoder.Reset(); - - m_rightGroup.SetInverted(true); - - frc::SmartDashboard::PutData("Field", &m_fieldSim); - } - - static constexpr units::meters_per_second_t kMaxSpeed = - 3.0_mps; // 3 meters per second - static constexpr units::radians_per_second_t kMaxAngularSpeed{ - std::numbers::pi}; // 1/2 rotation per second - - void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); - void Drive(units::meters_per_second_t xSpeed, - units::radians_per_second_t rot); - void UpdateOdometry(); - void ResetOdometry(const frc::Pose2d& pose); - - frc::Pose2d GetPose() const { return m_poseEstimator.GetEstimatedPosition(); } - - void SimulationPeriodic(); - void Periodic(); - - private: - static constexpr units::meter_t kTrackWidth = 0.381_m * 2; - static constexpr double kWheelRadius = 0.0508; // meters - static constexpr int kEncoderResolution = 4096; - - frc::PWMSparkMax m_leftLeader{1}; - frc::PWMSparkMax m_leftFollower{2}; - frc::PWMSparkMax m_rightLeader{3}; - frc::PWMSparkMax m_rightFollower{4}; - - frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower}; - frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower}; - - frc::Encoder m_leftEncoder{0, 1}; - frc::Encoder m_rightEncoder{2, 3}; - - frc::PIDController m_leftPIDController{8.5, 0.0, 0.0}; - frc::PIDController m_rightPIDController{8.5, 0.0, 0.0}; - - frc::AnalogGyro m_gyro{0}; - - frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; - - frc::DifferentialDrivePoseEstimator m_poseEstimator{ - m_kinematics, m_gyro.GetRotation2d(), - units::meter_t{m_leftEncoder.GetDistance()}, - units::meter_t{m_rightEncoder.GetDistance()}, frc::Pose2d{}}; - - PhotonCameraWrapper m_pcw; - - // Gains are for example purposes only - must be determined for your own - // robot! - frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; - - // Simulation classes help us simulate our robot - frc::sim::AnalogGyroSim m_gyroSim{m_gyro}; - frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; - frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; - frc::Field2d m_fieldSim; - frc::LinearSystem<2, 2, 2> m_drivetrainSystem = - frc::LinearSystemId::IdentifyDrivetrainSystem( - 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); - frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ - m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; -}; diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h deleted file mode 100644 index b63362eee..000000000 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include -#include - -#include - -#include -#include - -class PhotonCameraWrapper { - private: - photon::PhotonCamera camera{"WPI2023"}; - - public: - photon::PhotonPoseEstimator m_poseEstimator{ - frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), - photon::MULTI_TAG_PNP_ON_RIO, frc::Transform3d{}}; - - inline std::optional Update( - frc::Pose2d estimatedPose) { - m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose)); - std::optional ret = std::nullopt; - for (const auto& change : camera.GetAllUnreadResults()) - ret = m_poseEstimator.Update(change); - - return ret; - } -}; diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h b/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h deleted file mode 100644 index 40bac7a42..000000000 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "Drivetrain.h" - -class Robot : public frc::TimedRobot { - public: - void RobotInit() override; - void RobotPeriodic() override; - void TeleopPeriodic() override; - void SimulationPeriodic() override; - - private: - frc::XboxController m_controller{0}; - - Drivetrain m_drive; -}; diff --git a/photonlib-cpp-examples/getinrange/.gitignore b/photonlib-cpp-examples/getinrange/.gitignore deleted file mode 100644 index 34878ab18..000000000 --- a/photonlib-cpp-examples/getinrange/.gitignore +++ /dev/null @@ -1 +0,0 @@ -vendordeps diff --git a/photonlib-cpp-examples/getinrange/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/getinrange/.wpilib/wpilib_preferences.json deleted file mode 100644 index 36bf1b5d2..000000000 --- a/photonlib-cpp-examples/getinrange/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": true, - "currentLanguage": "cpp", - "projectYear": "2024", - "teamNumber": 5 -} diff --git a/photonlib-cpp-examples/getinrange/build.gradle b/photonlib-cpp-examples/getinrange/build.gradle deleted file mode 100644 index 9ae88e157..000000000 --- a/photonlib-cpp-examples/getinrange/build.gradle +++ /dev/null @@ -1,116 +0,0 @@ -plugins { - id "cpp" - id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.3.2" - - id "com.dorongold.task-tree" version "2.1.0" -} - -repositories { - mavenLocal() - jcenter() -} - -apply from: "${rootDir}/../shared/examples_common.gradle" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcCpp - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Set to true to run simulation in debug mode -wpi.cpp.debugSimulation = false - -// Default enable simgui -wpi.sim.addGui().defaultEnabled = true -// Enable DS but not by default -wpi.sim.addDriverstation() - -model { - components { - frcUserProgram(NativeExecutableSpec) { - // We don't need to build for roborio -- if we do, we need to install - // a roborio toolchain every time we build in CI - // Ideally, we'd be able to set the roborio toolchain as optional, but - // I can't figure out how to set that environment variable from build.gradle - // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) - // for now, commented out - - // targetPlatform wpi.platforms.roborio - - if (includeDesktopSupport) { - targetPlatform wpi.platforms.desktop - } - - sources.cpp { - source { - srcDir 'src/main/cpp' - include '**/*.cpp', '**/*.cc' - } - exportedHeaders { - srcDir 'src/main/include' - } - } - - // Set deploy task to deploy this component - deployArtifact.component = it - - // Enable run tasks for this component - wpi.cpp.enableExternalTasks(it) - - // Enable simulation for this component - wpi.sim.enable(it) - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - } - } - testSuites { - frcUserProgramTest(GoogleTestTestSuiteSpec) { - testing $.components.frcUserProgram - - sources.cpp { - source { - srcDir 'src/test/cpp' - include '**/*.cpp' - } - } - - // Enable run tasks for this component - wpi.cpp.enableExternalTasks(it) - - wpi.cpp.vendor.cpp(it) - wpi.cpp.deps.wpilib(it) - wpi.cpp.deps.googleTest(it) - } - } -} diff --git a/photonlib-cpp-examples/getinrange/settings.gradle b/photonlib-cpp-examples/getinrange/settings.gradle deleted file mode 100644 index d8802bb46..000000000 --- a/photonlib-cpp-examples/getinrange/settings.gradle +++ /dev/null @@ -1,30 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -rootProject.name = 'getinrange' - -pluginManagement { - repositories { - mavenLocal() - jcenter() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} diff --git a/photonlib-cpp-examples/getinrange/simgui-window.json b/photonlib-cpp-examples/getinrange/simgui-window.json deleted file mode 100644 index 1d727b721..000000000 --- a/photonlib-cpp-examples/getinrange/simgui-window.json +++ /dev/null @@ -1,60 +0,0 @@ -{ - "MainWindow": { - "GLOBAL": { - "height": "720", - "maximized": "0", - "style": "0", - "userScale": "2", - "width": "1280", - "xpos": "3124", - "ypos": "324" - } - }, - "Window": { - "###FMS": { - "Collapsed": "0", - "Pos": "5,540", - "Size": "283,146" - }, - "###Joysticks": { - "Collapsed": "0", - "Pos": "250,465", - "Size": "796,155" - }, - "###Keyboard 0 Settings": { - "Collapsed": "0", - "Pos": "10,50", - "Size": "300,560" - }, - "###NetworkTables": { - "Collapsed": "0", - "Pos": "250,277", - "Size": "750,185" - }, - "###Other Devices": { - "Collapsed": "0", - "Pos": "1025,20", - "Size": "250,695" - }, - "###System Joysticks": { - "Collapsed": "0", - "Pos": "5,350", - "Size": "192,218" - }, - "###Timing": { - "Collapsed": "0", - "Pos": "5,150", - "Size": "135,127" - }, - "Debug##Default": { - "Collapsed": "0", - "Pos": "60,60", - "Size": "400,400" - }, - "Robot State": { - "Collapsed": "0", - "Pos": "5,20", - "Size": "92,99" - } - } -} diff --git a/photonlib-cpp-examples/getinrange/simgui.json b/photonlib-cpp-examples/getinrange/simgui.json deleted file mode 100644 index 449f4b1d6..000000000 --- a/photonlib-cpp-examples/getinrange/simgui.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo" - } - } -} diff --git a/photonlib-cpp-examples/getinrange/src/main/include/Robot.h b/photonlib-cpp-examples/getinrange/src/main/include/Robot.h deleted file mode 100644 index e8905e4e0..000000000 --- a/photonlib-cpp-examples/getinrange/src/main/include/Robot.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include - -class Robot : public frc::TimedRobot { - public: - void TeleopPeriodic() override; - - private: - // Constants such as camera and target height stored. Change per robot and - // goal! - const units::meter_t CAMERA_HEIGHT = 24_in; - const units::meter_t TARGET_HEIGHT = 5_ft; - - // Angle between horizontal and the camera. - const units::radian_t CAMERA_PITCH = 0_deg; - - // How far from the target we want to be - const units::meter_t GOAL_RANGE_METERS = 3_ft; - - // PID constants should be tuned per robot - const double P_GAIN = 0.1; - const double D_GAIN = 0.0; - frc::PIDController controller{P_GAIN, 0.0, D_GAIN}; - - // Change this to match the name of your camera - photon::PhotonCamera camera{"photonvision"}; - - frc::XboxController xboxController{0}; - - // Drive motors - frc::PWMVictorSPX leftMotor{0}; - frc::PWMVictorSPX rightMotor{1}; - frc::DifferentialDrive drive{leftMotor, rightMotor}; -}; diff --git a/photonlib-cpp-examples/getinrange/src/test/cpp/main.cpp b/photonlib-cpp-examples/getinrange/src/test/cpp/main.cpp deleted file mode 100644 index 031d1ce96..000000000 --- a/photonlib-cpp-examples/getinrange/src/test/cpp/main.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "gtest/gtest.h" - -int main(int argc, char** argv) { - HAL_Initialize(500, 0); - ::testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/photonlib-cpp-examples/apriltagExample/.gitignore b/photonlib-cpp-examples/poseest/.gitignore similarity index 100% rename from photonlib-cpp-examples/apriltagExample/.gitignore rename to photonlib-cpp-examples/poseest/.gitignore diff --git a/photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/poseest/.wpilib/wpilib_preferences.json similarity index 100% rename from photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json rename to photonlib-cpp-examples/poseest/.wpilib/wpilib_preferences.json diff --git a/photonlib-cpp-examples/apriltagExample/WPILib-License.md b/photonlib-cpp-examples/poseest/WPILib-License.md similarity index 100% rename from photonlib-cpp-examples/apriltagExample/WPILib-License.md rename to photonlib-cpp-examples/poseest/WPILib-License.md diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/poseest/build.gradle similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/build.gradle rename to photonlib-cpp-examples/poseest/build.gradle diff --git a/photonlib-cpp-examples/apriltagExample/gradlew b/photonlib-cpp-examples/poseest/gradlew similarity index 100% rename from photonlib-cpp-examples/apriltagExample/gradlew rename to photonlib-cpp-examples/poseest/gradlew diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/poseest/gradlew.bat similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat rename to photonlib-cpp-examples/poseest/gradlew.bat diff --git a/photonlib-cpp-examples/poseest/networktables.json b/photonlib-cpp-examples/poseest/networktables.json new file mode 100644 index 000000000..fe51488c7 --- /dev/null +++ b/photonlib-cpp-examples/poseest/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/apriltagExample/settings.gradle b/photonlib-cpp-examples/poseest/settings.gradle similarity index 100% rename from photonlib-cpp-examples/apriltagExample/settings.gradle rename to photonlib-cpp-examples/poseest/settings.gradle diff --git a/photonlib-java-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/poseest/simgui-ds.json similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/simgui-ds.json rename to photonlib-cpp-examples/poseest/simgui-ds.json diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/poseest/simgui-window.json similarity index 68% rename from photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json rename to photonlib-cpp-examples/poseest/simgui-window.json index 160588770..3baff34d0 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json +++ b/photonlib-cpp-examples/poseest/simgui-window.json @@ -17,38 +17,48 @@ "Window": { "###/SmartDashboard/VisionSystemSim-main/Sim Field": { "Collapsed": "0", - "Pos": "199,200", - "Size": "1342,628" + "Pos": "554,10", + "Size": "991,527" }, "###FMS": { "Collapsed": "0", - "Pos": "5,540", - "Size": "283,146" + "Pos": "12,604", + "Size": "202,214" }, "###Joysticks": { "Collapsed": "0", - "Pos": "359,95", - "Size": "796,240" + "Pos": "278,685", + "Size": "976,219" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "865,52", + "Pos": "353,479", "Size": "830,620" }, + "###NetworkTables Info": { + "Collapsed": "0", + "Pos": "315,527", + "Size": "750,145" + }, "###Other Devices": { "Collapsed": "0", "Pos": "1025,20", "Size": "250,695" }, + "###Plot <0>": { + "Collapsed": "0", + "Pos": "113,22", + "Size": "448,400" + }, "###System Joysticks": { "Collapsed": "0", "Pos": "5,350", - "Size": "192,218" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,127" + "Size": "162,142" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +68,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "109,134" } } } diff --git a/photonlib-cpp-examples/poseest/simgui.json b/photonlib-cpp-examples/poseest/simgui.json new file mode 100644 index 000000000..b3f0626c3 --- /dev/null +++ b/photonlib-cpp-examples/poseest/simgui.json @@ -0,0 +1,161 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, + "SmartDashboard": { + "VisionSystemSim-main": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)" + } + ] + } + ] + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp similarity index 74% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp rename to photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 450574095..75a4ff22c 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp @@ -32,6 +32,7 @@ void Robot::RobotInit() {} void Robot::RobotPeriodic() { + launcher.periodic(); drivetrain.Periodic(); auto visionEst = vision.GetEstimatedGlobalPose(); @@ -52,47 +53,35 @@ void Robot::DisabledPeriodic() { drivetrain.Stop(); } void Robot::DisabledExit() {} -void Robot::AutonomousInit() { - autoTimer.Restart(); +void Robot::AutonomousInit() {} + +void Robot::AutonomousPeriodic() {} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() { frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; drivetrain.ResetPose(pose, true); } -void Robot::AutonomousPeriodic() { - if (autoTimer.Get() < 10_s) { - drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false); - } else { - autoTimer.Stop(); - drivetrain.Stop(); - } -} - -void Robot::AutonomousExit() {} - -void Robot::TeleopInit() {} - void Robot::TeleopPeriodic() { - double forward = -controller.GetLeftY() * kDriveSpeed; - if (std::abs(forward) < 0.1) { - forward = 0; - } - forward = forwardLimiter.Calculate(forward); + // Calculate drivetrain commands from Joystick values + auto forward = + -1.0 * controller.GetLeftY() * constants::Swerve::kMaxLinearSpeed; + auto strafe = + -1.0 * controller.GetLeftX() * constants::Swerve::kMaxLinearSpeed; + auto turn = + -1.0 * controller.GetRightX() * constants::Swerve::kMaxAngularSpeed; - double strafe = -controller.GetLeftX() * kDriveSpeed; - if (std::abs(strafe) < 0.1) { - strafe = 0; - } - strafe = strafeLimiter.Calculate(strafe); + // Command drivetrain motors based on target speeds + drivetrain.Drive(forward, strafe, turn); - double turn = -controller.GetRightX() * kDriveSpeed; - if (std::abs(turn) < 0.1) { - turn = 0; - } - turn = turnLimiter.Calculate(turn); - - drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed, - strafe * constants::Swerve::kMaxLinearSpeed, - turn * constants::Swerve::kMaxAngularSpeed, true); + // Calculate whether the gamepiece launcher runs based on our global pose + // estimate. + frc::Pose2d curPose = drivetrain.GetPose(); + bool shouldRun = (curPose.Y() > 2.0_m && + curPose.X() < 4.0_m); // Close enough to blue speaker + launcher.setRunning(shouldRun); } void Robot::TeleopExit() {} @@ -104,6 +93,7 @@ void Robot::TestPeriodic() {} void Robot::TestExit() {} void Robot::SimulationPeriodic() { + launcher.simulationPeriodic(); drivetrain.SimulationPeriodic(); vision.SimPeriodic(drivetrain.GetSimPose()); diff --git a/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp similarity index 50% rename from photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp rename to photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp index 7ac4cd93b..fc61a731f 100644 --- a/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/GamepieceLauncher.cpp @@ -22,41 +22,37 @@ * SOFTWARE. */ -#include "Robot.h" +#include "subsystems/GamepieceLauncher.h" // Include the header file -#include - -void Robot::TeleopPeriodic() { - double forwardSpeed; - double rotationSpeed = xboxController.GetLeftX(); - - if (xboxController.GetAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - photon::PhotonPipelineResult result = camera.GetLatestResult(); - - if (result.HasTargets()) { - // First calculate range - units::meter_t range = photon::PhotonUtils::CalculateDistanceToTarget( - CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, - units::degree_t{result.GetBestTarget().GetPitch()}); - - // Use this range as the measurement we give to the PID controller. - forwardSpeed = - -controller.Calculate(range.value(), GOAL_RANGE_METERS.value()); - } else { - // If we have no targets, stay still. - forwardSpeed = 0; - } - } else { - // Manual Driver Mode - forwardSpeed = -xboxController.GetRightY(); - } - - // Use our forward/turn speeds to control the drivetrain - drive.ArcadeDrive(forwardSpeed, rotationSpeed); +GamepieceLauncher::GamepieceLauncher() { + motor = new frc::PWMSparkMax(8); // Create the motor object for PWM port 8 + simulationInit(); } -#ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } -#endif +void GamepieceLauncher::setRunning(bool shouldRun) { + curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0; +} + +void GamepieceLauncher::periodic() { + // Calculate the maximum RPM + double maxRPM = + units::radians_per_second_t(frc::DCMotor::Falcon500(1).freeSpeed) + .to() * + 60 / (2 * std::numbers::pi); + curMotorCmd = curDesSpd / maxRPM; + curMotorCmd = std::clamp(curMotorCmd, 0.0, 1.0); + motor->Set(curMotorCmd); + + frc::SmartDashboard::PutNumber("GPLauncher Des Spd (RPM)", curDesSpd); +} + +void GamepieceLauncher::simulationPeriodic() { + launcherSim.SetInputVoltage(curMotorCmd * + frc::RobotController::GetBatteryVoltage()); + launcherSim.Update(0.02_s); + auto spd = launcherSim.GetAngularVelocity().to() * 60 / + (2 * std::numbers::pi); + frc::SmartDashboard::PutNumber("GPLauncher Act Spd (RPM)", spd); +} + +void GamepieceLauncher::simulationInit() {} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp similarity index 97% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp rename to photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp index 54c39b1d5..154c2f88e 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDrive.cpp @@ -50,15 +50,15 @@ void SwerveDrive::Periodic() { void SwerveDrive::Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega, bool openLoop) { + units::radians_per_second_t omega) { frc::ChassisSpeeds newChassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); - SetChassisSpeeds(newChassisSpeeds, openLoop, false); + SetChassisSpeeds(newChassisSpeeds, true, false); } void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, bool openLoop, bool steerInPlace) { - SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop, + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), true, steerInPlace); this->targetChassisSpeeds = newChassisSpeeds; } @@ -75,7 +75,7 @@ void SwerveDrive::SetModuleStates( } } -void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s, true); } +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s); } void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp) { diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 000000000..31bbe09e6 --- /dev/null +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 000000000..785d9ca7d --- /dev/null +++ b/photonlib-cpp-examples/poseest/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,147 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "subsystems/SwerveModule.h" + +#include +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/poseest/src/main/deploy/example.txt b/photonlib-cpp-examples/poseest/src/main/deploy/example.txt new file mode 100644 index 000000000..15bc5c1eb --- /dev/null +++ b/photonlib-cpp-examples/poseest/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/poseest/src/main/include/Constants.h similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h rename to photonlib-cpp-examples/poseest/src/main/include/Constants.h diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h b/photonlib-cpp-examples/poseest/src/main/include/Robot.h similarity index 85% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h rename to photonlib-cpp-examples/poseest/src/main/include/Robot.h index 7a3697494..96af16d53 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Robot.h @@ -25,11 +25,10 @@ #pragma once #include -#include #include -#include #include "Vision.h" +#include "subsystems/GamepieceLauncher.h" #include "subsystems/SwerveDrive.h" class Robot : public frc::TimedRobot { @@ -53,10 +52,6 @@ class Robot : public frc::TimedRobot { private: SwerveDrive drivetrain{}; Vision vision{}; + GamepieceLauncher launcher{}; frc::XboxController controller{0}; - frc::SlewRateLimiter forwardLimiter{1.0 / 0.6_s}; - frc::SlewRateLimiter strafeLimiter{1.0 / 0.6_s}; - frc::SlewRateLimiter turnLimiter{1.0 / 0.33_s}; - frc::Timer autoTimer{}; - double kDriveSpeed{0.6}; }; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h similarity index 98% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h rename to photonlib-cpp-examples/poseest/src/main/include/Vision.h index 200bc43b5..af3713389 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Vision.h @@ -142,7 +142,7 @@ class Vision { constants::Vision::kTagLayout, photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, constants::Vision::kRobotToCam}; - photon::PhotonCamera camera{"photonvision"}; + photon::PhotonCamera camera{constants::Vision::kCameraName}; std::unique_ptr visionSim; std::unique_ptr cameraProp; std::shared_ptr cameraSim; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h new file mode 100644 index 000000000..39590267d --- /dev/null +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/GamepieceLauncher.h @@ -0,0 +1,66 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_ +#define PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +class GamepieceLauncher { + public: + GamepieceLauncher(); // Constructor + void setRunning(bool shouldRun); // Method to start/stop the launcher + void periodic(); // Method to handle periodic updates + void simulationPeriodic(); // Method to handle simulation updates + + private: + frc::PWMSparkMax* motor; // Motor controller + + const double LAUNCH_SPEED_RPM = 2500; + double curDesSpd = 0.0; + double curMotorCmd = 0.0; + + static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia = + 0.5 * 1.5_lb * 4_in * 4_in; + + frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(1); + frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem( + m_gearbox, kFlywheelMomentOfInertia, 1.0)}; + + frc::sim::FlywheelSim launcherSim{m_plant, m_gearbox, 1.0}; + + void simulationInit(); // Method to initialize simulation components +}; + +#endif // PHOTONVISION_PHOTONLIB_CPP_EXAMPLES_POSEEST_SRC_MAIN_INCLUDE_SUBSYSTEMS_GAMEPIECELAUNCHER_H_ diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h similarity index 98% rename from photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h rename to photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h index 1e3d26b32..1fb0bf7e8 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDrive.h @@ -38,7 +38,7 @@ class SwerveDrive { SwerveDrive(); void Periodic(); void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, - units::radians_per_second_t omega, bool openLoop); + units::radians_per_second_t omega); void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, bool openLoop, bool steerInPlace); void SetModuleStates( diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 000000000..c1cee3e6f --- /dev/null +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 000000000..444e4bdf4 --- /dev/null +++ b/photonlib-cpp-examples/poseest/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp b/photonlib-cpp-examples/poseest/src/test/cpp/main.cpp similarity index 100% rename from photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp rename to photonlib-cpp-examples/poseest/src/test/cpp/main.cpp diff --git a/photonlib-cpp-examples/poseest/swerve_module.png b/photonlib-cpp-examples/poseest/swerve_module.png new file mode 100644 index 000000000..25990c839 Binary files /dev/null and b/photonlib-cpp-examples/poseest/swerve_module.png differ diff --git a/photonlib-cpp-examples/poseest/tag-blue.png b/photonlib-cpp-examples/poseest/tag-blue.png new file mode 100644 index 000000000..04b9e4f7b Binary files /dev/null and b/photonlib-cpp-examples/poseest/tag-blue.png differ diff --git a/photonlib-cpp-examples/poseest/tag-green.png b/photonlib-cpp-examples/poseest/tag-green.png new file mode 100644 index 000000000..63029fcf2 Binary files /dev/null and b/photonlib-cpp-examples/poseest/tag-green.png differ diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore deleted file mode 100644 index 34878ab18..000000000 --- a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore +++ /dev/null @@ -1 +0,0 @@ -vendordeps diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json deleted file mode 100644 index 36bf1b5d2..000000000 --- a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": true, - "currentLanguage": "cpp", - "projectYear": "2024", - "teamNumber": 5 -} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md deleted file mode 100644 index 3d5a824ca..000000000 --- a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle deleted file mode 100644 index 44fbca751..000000000 --- a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle +++ /dev/null @@ -1,30 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -rootProject.name = 'aimattarget' - -pluginManagement { - repositories { - mavenLocal() - jcenter() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json deleted file mode 100644 index e1ba9acce..000000000 --- a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json +++ /dev/null @@ -1,57 +0,0 @@ -{ - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" - }, - "windows": { - "/SmartDashboard/VisionSystemSim-main/Sim Field": { - "EstimatedRobotModules": { - "arrows": false, - "image": "swerve_module.png", - "length": 0.30000001192092896, - "width": 0.30000001192092896 - }, - "apriltag": { - "image": "tag-green.png", - "length": 0.6000000238418579, - "width": 0.6000000238418579 - }, - "bottom": 544, - "builtin": "2023 Charged Up", - "cameras": { - "arrowSize": 19, - "arrowWeight": 1.0, - "style": "Hidden" - }, - "height": 8.013679504394531, - "left": 46, - "right": 1088, - "top": 36, - "visibleTargetPoses": { - "image": "tag-blue.png" - }, - "width": 16.541748046875, - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "transitory": { - "SmartDashboard": { - "Drive": { - "open": true - }, - "open": true - }, - "photonvision": { - "open": true, - "photonvision": { - "open": true - } - } - } - } -} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp deleted file mode 100644 index 031d1ce96..000000000 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "gtest/gtest.h" - -int main(int argc, char** argv) { - HAL_Initialize(500, 0); - ::testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/photonlib-java-examples/README.md b/photonlib-java-examples/README.md index d02d6d3a7..d2b96976e 100644 --- a/photonlib-java-examples/README.md +++ b/photonlib-java-examples/README.md @@ -1,8 +1,14 @@ ## PhotonLib Java Examples +All examples demonstrate controlling a swerve drive with outputs from PhotonVision. + +Simulation is available to demonstrate the concepts - swerve physics is approximated. + +You can access a stream of what the simulated camera sees by going to https://localhost:1182 . + ### Running examples -For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples). +For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#running-examples). --- @@ -10,36 +16,26 @@ For instructions on how to run these examples locally, see [Running Examples](ht A simple demonstration of using PhotonVision's 2d target yaw to align a differential drivetrain with a target. ---- - -### [**`getinrange`**](getinrange) - -A simple demonstration of using PhotonVision's 2d target pitch to bring a differential drivetrain to a specific distance from a target. +**Keyboard controls:** +- Translate field-relative: WASD +- Rotate counter/clockwise: Q/E +- Perform vision alignment: Z --- ### [**`aimandrange`**](aimandrange) -A combination of the previous `aimattarget` and `getinrange` examples to simultaneously aim and get in range of a target. - ---- - -### [**`simaimandrange`**](simaimandrange) - -The above `aimandrange` example with simulation support. - - +Extends`aimattarget` to add getting in range of the target. **Keyboard controls:** -- Drive forward/backward: W/S -- Turn left/right: A/D +- Translate field-relative: WASD +- Rotate counter/clockwise: Q/E - Perform vision alignment: Z --- -### [**`swervedriveposeestsim`**](swervedriveposeestsim) +### [**`poseest`**](poseest) -A minimal swerve drive example demonstrating the usage of PhotonVision for AprilTag vision estimation with a swerve drive pose estimator. The example also has simulation support with an approximation of swerve drive dynamics. @@ -50,4 +46,5 @@ The example also has simulation support with an approximation of swerve drive dy **Keyboard controls:** - Translate field-relative: WASD - Rotate counter/clockwise: Q/E +- Perform vision alignment: Z - Offset pose estimate: X diff --git a/photonlib-java-examples/aimandrange/.gitignore b/photonlib-java-examples/aimandrange/.gitignore index 9535c8303..9f76c3a54 100644 --- a/photonlib-java-examples/aimandrange/.gitignore +++ b/photonlib-java-examples/aimandrange/.gitignore @@ -158,5 +158,16 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json +networktables.json diff --git a/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json b/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json index c72767ad4..618039266 100644 --- a/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json +++ b/photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 6995 + "teamNumber": 4512 } diff --git a/photonlib-java-examples/aimandrange/README.md b/photonlib-java-examples/aimandrange/README.md index a0a24688a..d93ca3bb4 100644 --- a/photonlib-java-examples/aimandrange/README.md +++ b/photonlib-java-examples/aimandrange/README.md @@ -1,3 +1,3 @@ -## **`aimandrange`** +## **`aimattarget`** -### See [PhotonLib Java Examples](../README.md#aimandrange) +### See [PhotonLib Java Examples](../README.md#aimattarget) diff --git a/photonlib-java-examples/aimandrange/WPILib-License.md b/photonlib-java-examples/aimandrange/WPILib-License.md index 3d5a824ca..645e54253 100644 --- a/photonlib-java-examples/aimandrange/WPILib-License.md +++ b/photonlib-java-examples/aimandrange/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 8c7ef1e20..28dc34bca 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -10,6 +10,11 @@ apply from: "${rootDir}/../shared/examples_common.gradle" def ROBOT_MAIN_CLASS = "frc.robot.Main" +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.3.2" +wpi.versions.wpimathVersion = "2024.3.2" + + // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { @@ -19,7 +24,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) + team = project.frc.getTeamOrDefault(4512) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -48,7 +53,7 @@ wpi.java.debugJni = false def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. +// Also defines JUnit 5. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.jar b/photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.jar similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.jar rename to photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.jar diff --git a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties rename to photonlib-java-examples/aimandrange/gradle/wrapper/gradle-wrapper.properties diff --git a/photonlib-cpp-examples/getinrange/gradlew b/photonlib-java-examples/aimandrange/gradlew old mode 100755 new mode 100644 similarity index 100% rename from photonlib-cpp-examples/getinrange/gradlew rename to photonlib-java-examples/aimandrange/gradlew diff --git a/photonlib-java-examples/swervedriveposeestsim/gradlew.bat b/photonlib-java-examples/aimandrange/gradlew.bat similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/gradlew.bat rename to photonlib-java-examples/aimandrange/gradlew.bat diff --git a/photonlib-java-examples/aimandrange/settings.gradle b/photonlib-java-examples/aimandrange/settings.gradle index c48c299f7..091a37a02 100644 --- a/photonlib-java-examples/aimandrange/settings.gradle +++ b/photonlib-java-examples/aimandrange/settings.gradle @@ -1,7 +1,5 @@ import org.gradle.internal.os.OperatingSystem -rootProject.name = 'aimandrange' - pluginManagement { repositories { mavenLocal() diff --git a/photonlib-java-examples/aimandrange/simgui-ds.json b/photonlib-java-examples/aimandrange/simgui-ds.json index 69b1a3cbc..addd5860c 100644 --- a/photonlib-java-examples/aimandrange/simgui-ds.json +++ b/photonlib-java-examples/aimandrange/simgui-ds.json @@ -11,13 +11,16 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/photonlib-java-examples/aimandrange/simgui.json b/photonlib-java-examples/aimandrange/simgui.json index cc05c0f4c..fc015367d 100644 --- a/photonlib-java-examples/aimandrange/simgui.json +++ b/photonlib-java-examples/aimandrange/simgui.json @@ -2,17 +2,154 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, "SmartDashboard": { - "Field": { + "VisionSystemSim-main": { "open": true }, "open": true } } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables View": { + "visible": false + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 338, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/Vision Target Range (m)" + } + ] + } + ] + } } } diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java new file mode 100644 index 000000000..921f57a87 --- /dev/null +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Constants.java @@ -0,0 +1,144 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; + +public class Constants { + public static class Vision { + public static final String kCameraName = "YOUR CAMERA NAME"; + // Cam mounted facing forward, half a meter forward of center, half a meter up from center, + // pitched upward. + private static final double camPitch = Units.degreesToRadians(30.0); + public static final Transform3d kRobotToCam = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); + + // The layout of the AprilTags on the field + public static final AprilTagFieldLayout kTagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } + + public static class Swerve { + // Physical properties + public static final double kTrackWidth = Units.inchesToMeters(18.5); + public static final double kTrackLength = Units.inchesToMeters(18.5); + public static final double kRobotWidth = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kRobotLength = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kMaxLinearSpeed = Units.feetToMeters(15.5); + public static final double kMaxAngularSpeed = Units.rotationsToRadians(2); + public static final double kWheelDiameter = Units.inchesToMeters(4); + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + public static final double kDriveGearRatio = 6.75; // 6.75:1 SDS MK4 L2 ratio + public static final double kSteerGearRatio = 12.8; // 12.8:1 + + public static final double kDriveDistPerPulse = kWheelCircumference / 1024 / kDriveGearRatio; + public static final double kSteerRadPerPulse = 2 * Math.PI / 1024; + + public enum ModuleConstants { + FL( // Front left + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2), + FR( // Front Right + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2), + BL( // Back Left + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2), + BR( // Back Right + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2); + + public final int moduleNum; + public final int driveMotorID; + public final int driveEncoderA; + public final int driveEncoderB; + public final int steerMotorID; + public final int steerEncoderA; + public final int steerEncoderB; + public final double angleOffset; + public final Translation2d centerOffset; + + private ModuleConstants( + int moduleNum, + int driveMotorID, + int driveEncoderA, + int driveEncoderB, + int steerMotorID, + int steerEncoderA, + int steerEncoderB, + double angleOffset, + double xOffset, + double yOffset) { + this.moduleNum = moduleNum; + this.driveMotorID = driveMotorID; + this.driveEncoderA = driveEncoderA; + this.driveEncoderB = driveEncoderB; + this.steerMotorID = steerMotorID; + this.steerEncoderA = steerEncoderA; + this.steerEncoderB = steerEncoderB; + this.angleOffset = angleOffset; + centerOffset = new Translation2d(xOffset, yOffset); + } + } + + // Feedforward + // Linear drive feed forward + public static final SimpleMotorFeedforward kDriveFF = + new SimpleMotorFeedforward( // real + 0.25, // Voltage to break static friction + 2.5, // Volts per meter per second + 0.3 // Volts per meter per second squared + ); + // Steer feed forward + public static final SimpleMotorFeedforward kSteerFF = + new SimpleMotorFeedforward( // real + 0.5, // Voltage to break static friction + 0.25, // Volts per radian per second + 0.01 // Volts per radian per second squared + ); + + // PID + public static final double kDriveKP = 1; + public static final double kDriveKI = 0; + public static final double kDriveKD = 0; + + public static final double kSteerKP = 20; + public static final double kSteerKI = 0; + public static final double kSteerKD = 0.25; + } +} diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java index 077f979f9..f227c28da 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Main.java @@ -26,19 +26,9 @@ package frc.robot; import edu.wpi.first.wpilibj.RobotBase; -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ public final class Main { private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ public static void main(String... args) { RobotBase.startRobot(Robot::new); } 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 d7a0c2a23..0d1429c6c 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 @@ -24,88 +24,139 @@ package frc.robot; -import edu.wpi.first.math.controller.PIDController; +import static frc.robot.Constants.Vision.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.drivetrain.SwerveDrive; import org.photonvision.PhotonCamera; import org.photonvision.PhotonUtils; -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ public class Robot extends TimedRobot { - // Constants such as camera and target height stored. Change per robot and goal! - final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); - final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); - // Angle between horizontal and the camera. - final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + private SwerveDrive drivetrain; + private VisionSim visionSim; + private PhotonCamera camera; - // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(3); + private final double VISION_TURN_kP = 0.01; + 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; - // Change this to match the name of your camera - PhotonCamera camera = new PhotonCamera("photonvision"); + private XboxController controller; - // PID constants should be tuned per robot - final double LINEAR_P = 0.1; - final double LINEAR_D = 0.0; - PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); + @Override + public void robotInit() { + drivetrain = new SwerveDrive(); + camera = new PhotonCamera(kCameraName); - final double ANGULAR_P = 0.1; - final double ANGULAR_D = 0.0; - PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); + visionSim = new VisionSim(camera); - XboxController xboxController = new XboxController(0); + controller = new XboxController(0); + } - // Drive motors - PWMVictorSPX leftMotor = new PWMVictorSPX(0); - PWMVictorSPX rightMotor = new PWMVictorSPX(1); - DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + @Override + public void robotPeriodic() { + // Update drivetrain subsystem + drivetrain.periodic(); + + // Log values to the dashboard + drivetrain.log(); + } + + @Override + public void disabledPeriodic() { + drivetrain.stop(); + } + + @Override + public void teleopInit() { + resetPose(); + } @Override public void teleopPeriodic() { - double forwardSpeed; - double rotationSpeed; - - if (xboxController.getAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - var result = camera.getLatestResult(); + // Calculate drivetrain commands from Joystick values + double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed; + double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed; + double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed; + // Read in relevant data from the Camera + boolean targetVisible = false; + double targetYaw = 0.0; + double targetRange = 0.0; + var results = camera.getAllUnreadResults(); + if (!results.isEmpty()) { + // Camera processed a new frame since last + // Get the last one in the list. + var result = results.get(results.size() - 1); if (result.hasTargets()) { - // First calculate range - double range = - PhotonUtils.calculateDistanceToTargetMeters( - CAMERA_HEIGHT_METERS, - TARGET_HEIGHT_METERS, - CAMERA_PITCH_RADIANS, - Units.degreesToRadians(result.getBestTarget().getPitch())); + // At least one AprilTag was seen by the camera + for (var target : result.getTargets()) { + if (target.getFiducialId() == 7) { + // Found Tag 7, record its information + targetYaw = target.getYaw(); + targetRange = + PhotonUtils.calculateDistanceToTargetMeters( + 0.5, // Measured with a tape measure, or in CAD. + 1.435, // From 2024 game manual for ID 7 + Units.degreesToRadians(-30.0), // Measured with a protractor, or in CAD. + Units.degreesToRadians(target.getPitch())); - // Use this range as the measurement we give to the PID controller. - // -1.0 required to ensure positive PID controller effort _increases_ range - forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS); - - // Also calculate angular power - // -1.0 required to ensure positive PID controller effort _increases_ yaw - rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0); - } else { - // If we have no targets, stay still. - forwardSpeed = 0; - rotationSpeed = 0; + targetVisible = true; + } + } } - } else { - // Manual Driver Mode - forwardSpeed = -xboxController.getRightY(); - rotationSpeed = xboxController.getLeftX(); } - // Use our forward/turn speeds to control the drivetrain - drive.arcadeDrive(forwardSpeed, rotationSpeed); + // Auto-align when requested + if (controller.getAButton() && 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 fwd/rev command with an automatic one + // That turns toward the tag, and gets the range right. + turn = + (VISION_DES_ANGLE_deg - targetYaw) * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; + forward = + (VISION_DES_RANGE_m - targetRange) * VISION_STRAFE_kP * Constants.Swerve.kMaxLinearSpeed; + } + + // Command drivetrain motors based on target speeds + drivetrain.drive(forward, strafe, turn); + + // Put debug information to the dashboard + SmartDashboard.putBoolean("Vision Target Visible", targetVisible); + SmartDashboard.putNumber("Vision Target Range (m)", targetRange); + } + + @Override + public void simulationPeriodic() { + // Update drivetrain simulation + drivetrain.simulationPeriodic(); + + // Update camera simulation + visionSim.simulationPeriodic(drivetrain.getSimPose()); + + var debugField = visionSim.getSimDebugField(); + debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose()); + debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); + + // Calculate battery voltage sag due to current draw + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); + } + + public void resetPose() { + // Example Only - startPose should be derived from some assumption + // of where your robot was placed on the field. + // The first pose in an autonomous path is often a good choice. + var startPose = new Pose2d(1, 1, new Rotation2d()); + drivetrain.resetPose(startPose, true); + visionSim.resetSimPose(startPose); } } diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java new file mode 100644 index 000000000..ba9d9d01c --- /dev/null +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/VisionSim.java @@ -0,0 +1,82 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot; + +import static frc.robot.Constants.Vision.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +public class VisionSim { + // Simulation + private PhotonCameraSim cameraSim; + private VisionSystemSim visionSim; + + public VisionSim(PhotonCamera cam_in) { + // ----- Simulation + if (Robot.isSimulation()) { + // Create the vision system simulation which handles cameras and targets on the field. + visionSim = new VisionSystemSim("main"); + // Add all the AprilTags inside the tag layout as visible targets to this simulated field. + visionSim.addAprilTags(kTagLayout); + // Create simulated camera properties. These can be set to mimic your actual camera. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(320, 240, Rotation2d.fromDegrees(90)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(70); + cameraProp.setAvgLatencyMs(30); + cameraProp.setLatencyStdDevMs(10); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + cameraSim = new PhotonCameraSim(cam_in, cameraProp); + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(cameraSim, kRobotToCam); + + cameraSim.enableDrawWireframe(true); + } + } + + // ----- Simulation + + public void simulationPeriodic(Pose2d robotSimPose) { + visionSim.update(robotSimPose); + } + + /** Reset pose history of the robot in the vision system simulation. */ + public void resetSimPose(Pose2d pose) { + if (Robot.isSimulation()) visionSim.resetRobotPose(pose); + } + + /** A Field2d for visualizing our robot and objects on the field. */ + public Field2d getSimDebugField() { + if (!Robot.isSimulation()) return null; + return visionSim.getDebugField(); + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java similarity index 98% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java rename to photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 21911c997..b41017a3e 100644 --- a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -120,13 +120,11 @@ public class SwerveDrive { * @param vxMeters X velocity (forwards/backwards) * @param vyMeters Y velocity (strafe left/right) * @param omegaRadians Angular velocity (rotation CCW+) - * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback - * control. */ - public void drive(double vxMeters, double vyMeters, double omegaRadians, boolean openLoop) { + public void drive(double vxMeters, double vyMeters, double omegaRadians) { var targetChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); - setChassisSpeeds(targetChassisSpeeds, openLoop, false); + setChassisSpeeds(targetChassisSpeeds, true, false); } /** @@ -162,7 +160,7 @@ public class SwerveDrive { /** Stop the swerve drive. */ public void stop() { - drive(0, 0, 0, true); + drive(0, 0, 0); } /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */ diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java rename to photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java rename to photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java diff --git a/photonlib-java-examples/aimandrange/swerve_module.png b/photonlib-java-examples/aimandrange/swerve_module.png new file mode 100644 index 000000000..25990c839 Binary files /dev/null and b/photonlib-java-examples/aimandrange/swerve_module.png differ diff --git a/photonlib-java-examples/aimandrange/tag-blue.png b/photonlib-java-examples/aimandrange/tag-blue.png new file mode 100644 index 000000000..04b9e4f7b Binary files /dev/null and b/photonlib-java-examples/aimandrange/tag-blue.png differ diff --git a/photonlib-java-examples/aimandrange/tag-green.png b/photonlib-java-examples/aimandrange/tag-green.png new file mode 100644 index 000000000..63029fcf2 Binary files /dev/null and b/photonlib-java-examples/aimandrange/tag-green.png differ diff --git a/photonlib-java-examples/aimattarget/.gitignore b/photonlib-java-examples/aimattarget/.gitignore index 9535c8303..9f76c3a54 100644 --- a/photonlib-java-examples/aimattarget/.gitignore +++ b/photonlib-java-examples/aimattarget/.gitignore @@ -158,5 +158,16 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json +networktables.json diff --git a/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json b/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json index c72767ad4..618039266 100644 --- a/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json +++ b/photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2024", - "teamNumber": 6995 + "teamNumber": 4512 } diff --git a/photonlib-java-examples/aimattarget/WPILib-License.md b/photonlib-java-examples/aimattarget/WPILib-License.md index 3d5a824ca..645e54253 100644 --- a/photonlib-java-examples/aimattarget/WPILib-License.md +++ b/photonlib-java-examples/aimattarget/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index fe6e93ab8..28dc34bca 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -6,19 +6,14 @@ plugins { sourceCompatibility = JavaVersion.VERSION_11 targetCompatibility = JavaVersion.VERSION_11 -def ROBOT_MAIN_CLASS = "frc.robot.Main" - apply from: "${rootDir}/../shared/examples_common.gradle" -ext { - wpilibVersion = "2025.0.0-alpha-1" - wpimathVersion = wpilibVersion - openCVversion = "4.8.0-2" -} +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.3.2" +wpi.versions.wpimathVersion = "2024.3.2" -wpi.getVersions().getOpencvVersion().convention(openCVversion); -wpi.getVersions().getWpilibVersion().convention(wpilibVersion); -wpi.getVersions().getWpimathVersion().convention(wpimathVersion); // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. @@ -29,7 +24,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) + team = project.frc.getTeamOrDefault(4512) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -58,7 +53,7 @@ wpi.java.debugJni = false def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. +// Also defines JUnit 5. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.jar b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 000000000..249e5832f Binary files /dev/null and b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.jar differ diff --git a/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 000000000..ca8f28abd --- /dev/null +++ b/photonlib-java-examples/aimattarget/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-java-examples/aimattarget/gradlew similarity index 100% rename from photonlib-cpp-examples/swervedriveposeestsim/gradlew rename to photonlib-java-examples/aimattarget/gradlew diff --git a/photonlib-cpp-examples/apriltagExample/gradlew.bat b/photonlib-java-examples/aimattarget/gradlew.bat similarity index 96% rename from photonlib-cpp-examples/apriltagExample/gradlew.bat rename to photonlib-java-examples/aimattarget/gradlew.bat index 53a6b238d..f127cfd49 100644 --- a/photonlib-cpp-examples/apriltagExample/gradlew.bat +++ b/photonlib-java-examples/aimattarget/gradlew.bat @@ -1,91 +1,91 @@ -@rem -@rem Copyright 2015 the original author or authors. -@rem -@rem Licensed under the Apache License, Version 2.0 (the "License"); -@rem you may not use this file except in compliance with the License. -@rem You may obtain a copy of the License at -@rem -@rem https://www.apache.org/licenses/LICENSE-2.0 -@rem -@rem Unless required by applicable law or agreed to in writing, software -@rem distributed under the License is distributed on an "AS IS" BASIS, -@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -@rem See the License for the specific language governing permissions and -@rem limitations under the License. -@rem - -@if "%DEBUG%"=="" @echo off -@rem ########################################################################## -@rem -@rem Gradle startup script for Windows -@rem -@rem ########################################################################## - -@rem Set local scope for the variables with windows NT shell -if "%OS%"=="Windows_NT" setlocal - -set DIRNAME=%~dp0 -if "%DIRNAME%"=="" set DIRNAME=. -set APP_BASE_NAME=%~n0 -set APP_HOME=%DIRNAME% - -@rem Resolve any "." and ".." in APP_HOME to make it shorter. -for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi - -@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" - -@rem Find java.exe -if defined JAVA_HOME goto findJavaFromJavaHome - -set JAVA_EXE=java.exe -%JAVA_EXE% -version >NUL 2>&1 -if %ERRORLEVEL% equ 0 goto execute - -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:findJavaFromJavaHome -set JAVA_HOME=%JAVA_HOME:"=% -set JAVA_EXE=%JAVA_HOME%/bin/java.exe - -if exist "%JAVA_EXE%" goto execute - -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:execute -@rem Setup the command line - -set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar - - -@rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* - -:end -@rem End local scope for the variables with windows NT shell -if %ERRORLEVEL% equ 0 goto mainEnd - -:fail -rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of -rem the _cmd.exe /c_ return code! -set EXIT_CODE=%ERRORLEVEL% -if %EXIT_CODE% equ 0 set EXIT_CODE=1 -if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% -exit /b %EXIT_CODE% - -:mainEnd -if "%OS%"=="Windows_NT" endlocal - -:omega +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-java-examples/aimattarget/settings.gradle b/photonlib-java-examples/aimattarget/settings.gradle index df1054244..091a37a02 100644 --- a/photonlib-java-examples/aimattarget/settings.gradle +++ b/photonlib-java-examples/aimattarget/settings.gradle @@ -1,7 +1,5 @@ import org.gradle.internal.os.OperatingSystem -rootProject.name = 'aimattarget' - pluginManagement { repositories { mavenLocal() diff --git a/photonlib-java-examples/aimattarget/simgui-ds.json b/photonlib-java-examples/aimattarget/simgui-ds.json index 69b1a3cbc..addd5860c 100644 --- a/photonlib-java-examples/aimattarget/simgui-ds.json +++ b/photonlib-java-examples/aimattarget/simgui-ds.json @@ -11,13 +11,16 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/photonlib-java-examples/aimattarget/simgui.json b/photonlib-java-examples/aimattarget/simgui.json index cc05c0f4c..a1406290b 100644 --- a/photonlib-java-examples/aimattarget/simgui.json +++ b/photonlib-java-examples/aimattarget/simgui.json @@ -2,17 +2,152 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobot": { + "arrowWeight": 3.0, + "length": 0.800000011920929, + "selectable": false, + "weight": 3.0, + "width": 0.800000011920929 + }, + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "selectable": false, + "width": 0.30000001192092896 + }, + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "length": 0.800000011920929, + "selectable": false, + "weight": 2.0, + "width": 0.800000011920929 + }, + "VisionEstimation": { + "arrowColor": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "arrowWeight": 2.0, + "color": [ + 0.0, + 0.6075949668884277, + 1.0, + 255.0 + ], + "selectable": false, + "weight": 2.0 + }, + "apriltag": { + "arrows": false, + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.5 + }, + "bottom": 1476, + "cameras": { + "arrowColor": [ + 0.29535865783691406, + 1.0, + 0.9910804033279419, + 255.0 + ], + "arrowSize": 19, + "arrowWeight": 3.0, + "length": 1.0, + "style": "Hidden", + "weight": 1.0, + "width": 1.0 + }, + "height": 8.210550308227539, + "image": "2023-field.png", + "left": 150, + "right": 2961, + "top": 79, + "visibleTargetPoses": { + "arrows": false, + "image": "tag-blue.png", + "length": 0.5, + "selectable": false, + "width": 0.4000000059604645 + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, "SmartDashboard": { - "Field": { + "VisionSystemSim-main": { "open": true }, "open": true } } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables View": { + "visible": false + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/Vision Target Visible" + } + ] + } + ], + "window": { + "visible": false + } + } } } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java new file mode 100644 index 000000000..921f57a87 --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Constants.java @@ -0,0 +1,144 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; + +public class Constants { + public static class Vision { + public static final String kCameraName = "YOUR CAMERA NAME"; + // Cam mounted facing forward, half a meter forward of center, half a meter up from center, + // pitched upward. + private static final double camPitch = Units.degreesToRadians(30.0); + public static final Transform3d kRobotToCam = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); + + // The layout of the AprilTags on the field + public static final AprilTagFieldLayout kTagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } + + public static class Swerve { + // Physical properties + public static final double kTrackWidth = Units.inchesToMeters(18.5); + public static final double kTrackLength = Units.inchesToMeters(18.5); + public static final double kRobotWidth = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kRobotLength = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kMaxLinearSpeed = Units.feetToMeters(15.5); + public static final double kMaxAngularSpeed = Units.rotationsToRadians(2); + public static final double kWheelDiameter = Units.inchesToMeters(4); + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + public static final double kDriveGearRatio = 6.75; // 6.75:1 SDS MK4 L2 ratio + public static final double kSteerGearRatio = 12.8; // 12.8:1 + + public static final double kDriveDistPerPulse = kWheelCircumference / 1024 / kDriveGearRatio; + public static final double kSteerRadPerPulse = 2 * Math.PI / 1024; + + public enum ModuleConstants { + FL( // Front left + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2), + FR( // Front Right + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2), + BL( // Back Left + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2), + BR( // Back Right + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2); + + public final int moduleNum; + public final int driveMotorID; + public final int driveEncoderA; + public final int driveEncoderB; + public final int steerMotorID; + public final int steerEncoderA; + public final int steerEncoderB; + public final double angleOffset; + public final Translation2d centerOffset; + + private ModuleConstants( + int moduleNum, + int driveMotorID, + int driveEncoderA, + int driveEncoderB, + int steerMotorID, + int steerEncoderA, + int steerEncoderB, + double angleOffset, + double xOffset, + double yOffset) { + this.moduleNum = moduleNum; + this.driveMotorID = driveMotorID; + this.driveEncoderA = driveEncoderA; + this.driveEncoderB = driveEncoderB; + this.steerMotorID = steerMotorID; + this.steerEncoderA = steerEncoderA; + this.steerEncoderB = steerEncoderB; + this.angleOffset = angleOffset; + centerOffset = new Translation2d(xOffset, yOffset); + } + } + + // Feedforward + // Linear drive feed forward + public static final SimpleMotorFeedforward kDriveFF = + new SimpleMotorFeedforward( // real + 0.25, // Voltage to break static friction + 2.5, // Volts per meter per second + 0.3 // Volts per meter per second squared + ); + // Steer feed forward + public static final SimpleMotorFeedforward kSteerFF = + new SimpleMotorFeedforward( // real + 0.5, // Voltage to break static friction + 0.25, // Volts per radian per second + 0.01 // Volts per radian per second squared + ); + + // PID + public static final double kDriveKP = 1; + public static final double kDriveKI = 0; + public static final double kDriveKD = 0; + + public static final double kSteerKP = 20; + public static final double kSteerKI = 0; + public static final double kSteerKD = 0.25; + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java index 077f979f9..f227c28da 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Main.java @@ -26,19 +26,9 @@ package frc.robot; import edu.wpi.first.wpilibj.RobotBase; -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ public final class Main { private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java index d4ff02267..4a79bba13 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java @@ -24,87 +24,121 @@ package frc.robot; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; +import static frc.robot.Constants.Vision.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.drivetrain.SwerveDrive; import org.photonvision.PhotonCamera; -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ public class Robot extends TimedRobot { - // Constants such as camera and target height stored. Change per robot and goal! - final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); - final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); - // Angle between horizontal and the camera. - final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); + private SwerveDrive drivetrain; + private VisionSim visionSim; + private PhotonCamera camera; - // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(3); + private final double VISION_TURN_kP = 0.01; - // Change this to match the name of your camera as shown in the web UI - PhotonCamera camera = new PhotonCamera("Microsoft_LifeCam_HD-3000"); + private XboxController controller; - // PID constants should be tuned per robot - final double LINEAR_P = 0.1; - final double LINEAR_D = 0.0; - PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); + @Override + public void robotInit() { + drivetrain = new SwerveDrive(); + camera = new PhotonCamera(kCameraName); - final double ANGULAR_P = 0.1; - final double ANGULAR_D = 0.0; - PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); + visionSim = new VisionSim(camera); - XboxController xboxController = new XboxController(0); - - // Drive motors - PWMVictorSPX leftMotor = new PWMVictorSPX(0); - PWMVictorSPX rightMotor = new PWMVictorSPX(1); - DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + controller = new XboxController(0); + } @Override public void robotPeriodic() { - var start = Timer.getFPGATimestamp(); - var res = camera.getLatestResult(); - var end = Timer.getFPGATimestamp(); - System.out.println( - "dt: " + (int) ((end - start) * 1e6) + "uS for targets: " + res.getTargets().size()); - SmartDashboard.putNumber("decodeTime", (int) ((end - start) * 1e6)); + // Update drivetrain subsystem + drivetrain.periodic(); + + // Log values to the dashboard + drivetrain.log(); + } + + @Override + public void disabledPeriodic() { + drivetrain.stop(); + } + + @Override + public void teleopInit() { + resetPose(); } @Override public void teleopPeriodic() { - double forwardSpeed; - double rotationSpeed; - - forwardSpeed = -xboxController.getRightY(); - - if (xboxController.getAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - var result = camera.getLatestResult(); + // Calculate drivetrain commands from Joystick values + double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed; + double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed; + double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed; + // Read in relevant data from the Camera + boolean targetVisible = false; + double targetYaw = 0.0; + var results = camera.getAllUnreadResults(); + if (!results.isEmpty()) { + // Camera processed a new frame since last + // Get the last one in the list. + var result = results.get(results.size() - 1); if (result.hasTargets()) { - // Calculate angular turn power - // -1.0 required to ensure positive PID controller effort _increases_ yaw - rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0); - } else { - // If we have no targets, stay still. - rotationSpeed = 0; + // At least one AprilTag was seen by the camera + for (var target : result.getTargets()) { + if (target.getFiducialId() == 7) { + // Found Tag 7, record its information + targetYaw = target.getYaw(); + targetVisible = true; + } + } } - } else { - // Manual Driver Mode - rotationSpeed = xboxController.getLeftX(); } - // Use our forward/turn speeds to control the drivetrain - drive.arcadeDrive(forwardSpeed, rotationSpeed); + // Auto-align when requested + if (controller.getAButton() && 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. + turn = -1.0 * targetYaw * VISION_TURN_kP * Constants.Swerve.kMaxAngularSpeed; + } + + // Command drivetrain motors based on target speeds + drivetrain.drive(forward, strafe, turn); + + // Put debug information to the dashboard + SmartDashboard.putBoolean("Vision Target Visible", targetVisible); + } + + @Override + public void simulationPeriodic() { + // Update drivetrain simulation + drivetrain.simulationPeriodic(); + + // Update camera simulation + visionSim.simulationPeriodic(drivetrain.getSimPose()); + + var debugField = visionSim.getSimDebugField(); + debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose()); + debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); + + // Calculate battery voltage sag due to current draw + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); + } + + public void resetPose() { + // Example Only - startPose should be derived from some assumption + // of where your robot was placed on the field. + // The first pose in an autonomous path is often a good choice. + var startPose = new Pose2d(1, 1, new Rotation2d()); + drivetrain.resetPose(startPose, true); + visionSim.resetSimPose(startPose); } } diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java new file mode 100644 index 000000000..ba9d9d01c --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/VisionSim.java @@ -0,0 +1,82 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot; + +import static frc.robot.Constants.Vision.*; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +public class VisionSim { + // Simulation + private PhotonCameraSim cameraSim; + private VisionSystemSim visionSim; + + public VisionSim(PhotonCamera cam_in) { + // ----- Simulation + if (Robot.isSimulation()) { + // Create the vision system simulation which handles cameras and targets on the field. + visionSim = new VisionSystemSim("main"); + // Add all the AprilTags inside the tag layout as visible targets to this simulated field. + visionSim.addAprilTags(kTagLayout); + // Create simulated camera properties. These can be set to mimic your actual camera. + var cameraProp = new SimCameraProperties(); + cameraProp.setCalibration(320, 240, Rotation2d.fromDegrees(90)); + cameraProp.setCalibError(0.35, 0.10); + cameraProp.setFPS(70); + cameraProp.setAvgLatencyMs(30); + cameraProp.setLatencyStdDevMs(10); + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + cameraSim = new PhotonCameraSim(cam_in, cameraProp); + // Add the simulated camera to view the targets on this simulated field. + visionSim.addCamera(cameraSim, kRobotToCam); + + cameraSim.enableDrawWireframe(true); + } + } + + // ----- Simulation + + public void simulationPeriodic(Pose2d robotSimPose) { + visionSim.update(robotSimPose); + } + + /** Reset pose history of the robot in the vision system simulation. */ + public void resetSimPose(Pose2d pose) { + if (Robot.isSimulation()) visionSim.resetRobotPose(pose); + } + + /** A Field2d for visualizing our robot and objects on the field. */ + public Field2d getSimDebugField() { + if (!Robot.isSimulation()) return null; + return visionSim.getDebugField(); + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java new file mode 100644 index 000000000..b41017a3e --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -0,0 +1,332 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.SPI.Port; +import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class SwerveDrive { + // Construct the swerve modules with their respective constants. + // The SwerveModule class will handle all the details of controlling the modules. + private final SwerveModule[] swerveMods = { + new SwerveModule(ModuleConstants.FL), + new SwerveModule(ModuleConstants.FR), + new SwerveModule(ModuleConstants.BL), + new SwerveModule(ModuleConstants.BR) + }; + + // The kinematics for translating between robot chassis speeds and module states. + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + swerveMods[0].getModuleConstants().centerOffset, + swerveMods[1].getModuleConstants().centerOffset, + swerveMods[2].getModuleConstants().centerOffset, + swerveMods[3].getModuleConstants().centerOffset); + + private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + + // The robot pose estimator for tracking swerve odometry and applying vision corrections. + private final SwerveDrivePoseEstimator poseEstimator; + + private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); + + // ----- Simulation + private final ADXRS450_GyroSim gyroSim; + private final SwerveDriveSim swerveDriveSim; + private double totalCurrentDraw = 0; + + public SwerveDrive() { + // Define the standard deviations for the pose estimator, which determine how fast the pose + // estimate converges to the vision measurement. This should depend on the vision measurement + // noise + // and how many or how frequently vision measurements are applied to the pose estimator. + var stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + var visionStdDevs = VecBuilder.fill(1, 1, 1); + poseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getGyroYaw(), + getModulePositions(), + new Pose2d(), + stateStdDevs, + visionStdDevs); + + // ----- Simulation + gyroSim = new ADXRS450_GyroSim(gyro); + swerveDriveSim = + new SwerveDriveSim( + kDriveFF, + DCMotor.getFalcon500(1), + kDriveGearRatio, + kWheelDiameter / 2.0, + kSteerFF, + DCMotor.getFalcon500(1), + kSteerGearRatio, + kinematics); + } + + public void periodic() { + for (SwerveModule module : swerveMods) { + module.periodic(); + } + + // Update the odometry of the swerve drive using the wheel encoders and gyro. + poseEstimator.update(getGyroYaw(), getModulePositions()); + } + + /** + * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to + * specific swerve module states. + * + * @param vxMeters X velocity (forwards/backwards) + * @param vyMeters Y velocity (strafe left/right) + * @param omegaRadians Angular velocity (rotation CCW+) + */ + public void drive(double vxMeters, double vyMeters, double omegaRadians) { + var targetChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + setChassisSpeeds(targetChassisSpeeds, true, false); + } + + /** + * Command the swerve drive to the desired chassis speeds by converting them to swerve module + * states and using {@link #setModuleStates(SwerveModuleState[], boolean)}. + * + * @param targetChassisSpeeds Target robot-relative chassis speeds (vx, vy, omega). + * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback + * control. + * @param steerInPlace If modules should steer to the target angle when target velocity is 0. + */ + public void setChassisSpeeds( + ChassisSpeeds targetChassisSpeeds, boolean openLoop, boolean steerInPlace) { + setModuleStates(kinematics.toSwerveModuleStates(targetChassisSpeeds), openLoop, steerInPlace); + this.targetChassisSpeeds = targetChassisSpeeds; + } + + /** + * Command the swerve modules to the desired states. Velocites exceeding the maximum speed will be + * desaturated (while preserving the ratios between modules). + * + * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback + * control. + * @param steerInPlace If modules should steer to the target angle when target velocity is 0. + */ + public void setModuleStates( + SwerveModuleState[] desiredStates, boolean openLoop, boolean steerInPlace) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, kMaxLinearSpeed); + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].setDesiredState(desiredStates[i], openLoop, steerInPlace); + } + } + + /** Stop the swerve drive. */ + public void stop() { + drive(0, 0, 0); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */ + public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */ + public void addVisionMeasurement( + Pose2d visionMeasurement, double timestampSeconds, Matrix stdDevs) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); + } + + /** + * Reset the estimated pose of the swerve drive on the field. + * + * @param pose New robot pose. + * @param resetSimPose If the simulated robot pose should also be reset. This effectively + * teleports the robot and should only be used during the setup of the simulation world. + */ + public void resetPose(Pose2d pose, boolean resetSimPose) { + if (resetSimPose) { + swerveDriveSim.reset(pose, false); + // we shouldnt realistically be resetting pose after startup, but we will handle it anyway for + // testing + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); + } + gyroSim.setAngle(-pose.getRotation().getDegrees()); + gyroSim.setRate(0); + } + + poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + /** Get the estimated pose of the swerve drive on the field. */ + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** The heading of the swerve drive's estimated pose on the field. */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** Raw gyro yaw (this may not match the field heading!). */ + public Rotation2d getGyroYaw() { + return gyro.getRotation2d(); + } + + /** Get the chassis speeds of the robot (vx, vy, omega) from the swerve module states. */ + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** + * Get the SwerveModuleState of each swerve module (speed, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + swerveMods[0].getState(), + swerveMods[1].getState(), + swerveMods[2].getState(), + swerveMods[3].getState() + }; + } + + /** + * Get the SwerveModulePosition of each swerve module (position, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + swerveMods[0].getPosition(), + swerveMods[1].getPosition(), + swerveMods[2].getPosition(), + swerveMods[3].getPosition() + }; + } + + /** + * Get the Pose2d of each swerve module based on kinematics and current robot pose. The returned + * array order matches the kinematics module order. + */ + public Pose2d[] getModulePoses() { + Pose2d[] modulePoses = new Pose2d[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + var module = swerveMods[i]; + modulePoses[i] = + getPose() + .transformBy( + new Transform2d( + module.getModuleConstants().centerOffset, module.getAbsoluteHeading())); + } + return modulePoses; + } + + /** Log various drivetrain values to the dashboard. */ + public void log() { + String table = "Drive/"; + Pose2d pose = getPose(); + SmartDashboard.putNumber(table + "X", pose.getX()); + SmartDashboard.putNumber(table + "Y", pose.getY()); + SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + + for (SwerveModule module : swerveMods) { + module.log(); + } + } + + // ----- Simulation + + public void simulationPeriodic() { + // Pass commanded motor voltages into swerve drive simulation + double[] driveInputs = new double[swerveMods.length]; + double[] steerInputs = new double[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + driveInputs[i] = swerveMods[i].getDriveVoltage(); + steerInputs[i] = swerveMods[i].getSteerVoltage(); + } + swerveDriveSim.setDriveInputs(driveInputs); + swerveDriveSim.setSteerInputs(steerInputs); + + // Simulate one timestep + swerveDriveSim.update(Robot.kDefaultPeriod); + + // Update module and gyro values with simulated values + var driveStates = swerveDriveSim.getDriveStates(); + var steerStates = swerveDriveSim.getSteerStates(); + totalCurrentDraw = 0; + double[] driveCurrents = swerveDriveSim.getDriveCurrentDraw(); + for (double current : driveCurrents) totalCurrentDraw += current; + double[] steerCurrents = swerveDriveSim.getSteerCurrentDraw(); + for (double current : steerCurrents) totalCurrentDraw += current; + for (int i = 0; i < swerveMods.length; i++) { + double drivePos = driveStates.get(i).get(0, 0); + double driveRate = driveStates.get(i).get(1, 0); + double steerPos = steerStates.get(i).get(0, 0); + double steerRate = steerStates.get(i).get(1, 0); + swerveMods[i].simulationUpdate( + drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); + } + + gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + } + + /** + * The "actual" pose of the robot on the field used in simulation. This is different from the + * swerve drive's estimated pose. + */ + public Pose2d getSimPose() { + return swerveDriveSim.getPose(); + } + + public double getCurrentDraw() { + return totalCurrentDraw; + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java new file mode 100644 index 000000000..85849141d --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -0,0 +1,495 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +/** + * This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users + * should first set inputs from motors with {@link #setDriveInputs(double...)} and {@link + * #setSteerInputs(double...)}, call {@link #update(double)} to update the simulation, and then set + * swerve module's encoder values and the drivetrain's gyro values with the results from this class. + * + *

In this class, distances are expressed with meters, angles with radians, and inputs with + * voltages. + * + *

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on + * the Sim GUI's field. + */ +public class SwerveDriveSim { + private final LinearSystem drivePlant; + private final double driveKs; + private final DCMotor driveMotor; + private final double driveGearing; + private final double driveWheelRadius; + private final LinearSystem steerPlant; + private final double steerKs; + private final DCMotor steerMotor; + private final double steerGearing; + + private final SwerveDriveKinematics kinematics; + private final int numModules; + + private final double[] driveInputs; + private final List> driveStates; + private final double[] steerInputs; + private final List> steerStates; + + private final Random rand = new Random(); + + // noiseless "actual" pose of the robot on the field + private Pose2d pose = new Pose2d(); + private double omegaRadsPerSec = 0; + + /** + * Creates a swerve drive simulation. + * + * @param driveFF The feedforward for the drive motors of this swerve drive. This should be in + * units of meters. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerFF The feedforward for the steer motors of this swerve drive. This should be in + * units of radians. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + SimpleMotorFeedforward driveFF, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + SimpleMotorFeedforward steerFF, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this( + new LinearSystem( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka), + VecBuilder.fill(0.0, 1.0 / driveFF.ka), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + VecBuilder.fill(0.0, 0.0)), + driveFF.ks, + driveMotor, + driveGearing, + driveWheelRadius, + new LinearSystem( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka), + VecBuilder.fill(0.0, 1.0 / steerFF.ka), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + VecBuilder.fill(0.0, 0.0)), + steerFF.ks, + steerMotor, + steerGearing, + kinematics); + } + + /** + * Creates a swerve drive simulation. + * + * @param drivePlant The {@link LinearSystem} representing a swerve module's drive system. The + * state should be in units of meters and input in volts. + * @param driveKs The static gain in volts of the drive system's feedforward, or the minimum + * voltage to cause motion. Set this to 0 to ignore static friction. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerPlant The {@link LinearSystem} representing a swerve module's steer system. The + * state should be in units of radians and input in volts. + * @param steerKs The static gain in volts of the steer system's feedforward, or the minimum + * voltage to cause motion. Set this to 0 to ignore static friction. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + LinearSystem drivePlant, + double driveKs, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + LinearSystem steerPlant, + double steerKs, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this.drivePlant = drivePlant; + this.driveKs = driveKs; + this.driveMotor = driveMotor; + this.driveGearing = driveGearing; + this.driveWheelRadius = driveWheelRadius; + this.steerPlant = steerPlant; + this.steerKs = steerKs; + this.steerMotor = steerMotor; + this.steerGearing = steerGearing; + + this.kinematics = kinematics; + numModules = kinematics.toSwerveModuleStates(new ChassisSpeeds()).length; + driveInputs = new double[numModules]; + driveStates = new ArrayList<>(numModules); + steerInputs = new double[numModules]; + steerStates = new ArrayList<>(numModules); + for (int i = 0; i < numModules; i++) { + driveStates.add(VecBuilder.fill(0, 0)); + steerStates.add(VecBuilder.fill(0, 0)); + } + } + + /** + * Sets the input voltages of the drive motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setDriveInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < driveInputs.length; i++) { + double input = inputs[i]; + driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Sets the input voltages of the steer motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setSteerInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < steerInputs.length; i++) { + double input = inputs[i]; + steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Computes the new x given the old x and the control input. Includes the effect of static + * friction. + * + * @param discA The discretized system matrix. + * @param discB The discretized input matrix. + * @param x The position/velocity state of the drive/steer system. + * @param input The input voltage. + * @param ks The kS value of the feedforward model. + * @return The updated x, including the effect of static friction. + */ + protected static Matrix calculateX( + Matrix discA, Matrix discB, Matrix x, double input, double ks) { + var Ax = discA.times(x); + double nextStateVel = Ax.get(1, 0); + // input required to make next state vel == 0 + double inputToStop = nextStateVel / -discB.get(1, 0); + // ks effect on system velocity + double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + + // after ks system effect: + nextStateVel += discB.get(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB.get(1, 0); + double signToStop = Math.signum(inputToStop); + double inputSign = Math.signum(input); + double ksInputEffect = 0; + + // system velocity was reduced to 0, resist any input + if (Math.abs(ksSystemEffect) < ks) { + double absInput = Math.abs(input); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + // non-zero system velocity, but input causes velocity sign change. Resist input after sign + // change + else if ((input * signToStop) > (inputToStop * signToStop)) { + double absInput = Math.abs(input - inputToStop); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + + // calculate next x including static friction + var Bu = discB.times(VecBuilder.fill(input + ksSystemEffect + ksInputEffect)); + return Ax.plus(Bu); + } + + /** + * Update the drivetrain states with the given timestep. + * + * @param dtSeconds The timestep in seconds. This should be the robot loop period. + */ + public void update(double dtSeconds) { + var driveDiscAB = Discretization.discretizeAB(drivePlant.getA(), drivePlant.getB(), dtSeconds); + var steerDiscAB = Discretization.discretizeAB(steerPlant.getA(), steerPlant.getB(), dtSeconds); + + var moduleDeltas = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates.get(i).get(0, 0); + driveStates.set( + i, + calculateX( + driveDiscAB.getFirst(), + driveDiscAB.getSecond(), + driveStates.get(i), + driveInputs[i], + driveKs)); + double currDriveStatePos = driveStates.get(i).get(0, 0); + steerStates.set( + i, + calculateX( + steerDiscAB.getFirst(), + steerDiscAB.getSecond(), + steerStates.get(i), + steerInputs[i], + steerKs)); + double currSteerStatePos = steerStates.get(i).get(0, 0); + moduleDeltas[i] = + new SwerveModulePosition( + currDriveStatePos - prevDriveStatePos, new Rotation2d(currSteerStatePos)); + } + + var twist = kinematics.toTwist2d(moduleDeltas); + pose = pose.exp(twist); + omegaRadsPerSec = twist.dtheta / dtSeconds; + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param preserveMotion If true, the current module states will be preserved. Otherwise, they + * will be reset to zeros. + */ + public void reset(Pose2d pose, boolean preserveMotion) { + this.pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates.set(i, VecBuilder.fill(0, 0)); + steerStates.set(i, VecBuilder.fill(0, 0)); + } + omegaRadsPerSec = 0; + } + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param moduleDriveStates The new states of the modules' drive systems in [meters, meters/sec]. + * These should match the module order used in the kinematics. + * @param moduleSteerStates The new states of the modules' steer systems in [radians, + * radians/sec]. These should match the module order used in the kinematics. + */ + public void reset( + Pose2d pose, List> moduleDriveStates, List> moduleSteerStates) { + if (moduleDriveStates.size() != driveStates.size() + || moduleSteerStates.size() != steerStates.size()) + throw new IllegalArgumentException("Provided module states do not match number of modules!"); + this.pose = pose; + for (int i = 0; i < numModules; i++) { + driveStates.set(i, moduleDriveStates.get(i).copy()); + steerStates.set(i, moduleSteerStates.get(i).copy()); + } + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + } + + /** + * Get the pose of the simulated swerve drive. Note that this is the "actual" pose of the robot in + * the simulation world, without any noise. If you are simulating a pose estimator, this pose + * should only be used for visualization or camera simulation. This should be called after {@link + * #update(double)}. + */ + public Pose2d getPose() { + return pose; + } + + /** + * Get the {@link SwerveModulePosition} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModulePosition[] getModulePositions() { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the {@link SwerveModulePosition} of each module with rudimentary noise simulation. The + * returned array order matches the kinematics module order. This should be called after {@link + * #update(double)}. + * + * @param driveStdDev The standard deviation in meters of the drive wheel position. + * @param steerStdDev The standard deviation in radians of the steer angle. + */ + public SwerveModulePosition[] getNoisyModulePositions(double driveStdDev, double steerStdDev) { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0) + rand.nextGaussian() * driveStdDev, + new Rotation2d(steerStates.get(i).get(0, 0) + rand.nextGaussian() * steerStdDev)); + } + return positions; + } + + /** + * Get the {@link SwerveModuleState} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModuleState[] getModuleStates() { + var positions = new SwerveModuleState[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModuleState( + driveStates.get(i).get(1, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the state of each module's drive system in [meters, meters/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getDriveStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < driveStates.size(); i++) { + states.add(driveStates.get(i).copy()); + } + return states; + } + + /** + * Get the state of each module's steer system in [radians, radians/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getSteerStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < steerStates.size(); i++) { + states.add(steerStates.get(i).copy()); + } + return states; + } + + /** + * Get the angular velocity of the robot, which can be useful for gyro simulation. CCW positive. + * This should be called after {@link #update(double)}. + */ + public double getOmegaRadsPerSec() { + return omegaRadsPerSec; + } + + /** + * Calculates the current drawn from the battery by the motor(s). Ignores regenerative current + * from back-emf. + * + * @param motor The motor(s) used. + * @param radiansPerSec The velocity of the motor in radians per second. + * @param inputVolts The voltage commanded by the motor controller (battery voltage * duty cycle). + * @param battVolts The voltage of the battery. + */ + protected static double getCurrentDraw( + DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { + double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + // ignore regeneration + if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); + else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + // calculate battery current + return (inputVolts / battVolts) * (effVolts / motor.rOhms); + } + + /** + * Get the current draw in amps for each module's drive motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getDriveCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = driveStates.get(i).get(1, 0) * driveGearing / driveWheelRadius; + currents[i] = + getCurrentDraw( + driveMotor, radiansPerSec, driveInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the current draw in amps for each module's steer motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getSteerCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = steerStates.get(i).get(1, 0) * steerGearing; + currents[i] = + getCurrentDraw( + steerMotor, radiansPerSec, steerInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the total current draw in amps of all swerve motors. This should be called after {@link + * #update(double)}. + */ + public double getTotalCurrentDraw() { + double sum = 0; + for (double val : getDriveCurrentDraw()) sum += val; + for (double val : getSteerCurrentDraw()) sum += val; + return sum; + } +} diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java new file mode 100644 index 000000000..4615d2921 --- /dev/null +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -0,0 +1,192 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + // --- Module Constants + private final ModuleConstants moduleConstants; + + // --- Hardware + private final PWMSparkMax driveMotor; + private final Encoder driveEncoder; + private final PWMSparkMax steerMotor; + private final Encoder steerEncoder; + + // --- Control + private SwerveModuleState desiredState = new SwerveModuleState(); + private boolean openLoop = false; + + // Simple PID feedback controllers run on the roborio + private PIDController drivePidController = new PIDController(kDriveKP, kDriveKI, kDriveKD); + // (A profiled steering PID controller may give better results by utilizing feedforward.) + private PIDController steerPidController = new PIDController(kSteerKP, kSteerKI, kSteerKD); + + // --- Simulation + private final EncoderSim driveEncoderSim; + private double driveCurrentSim = 0; + private final EncoderSim steerEncoderSim; + private double steerCurrentSim = 0; + + public SwerveModule(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + driveMotor = new PWMSparkMax(moduleConstants.driveMotorID); + driveEncoder = new Encoder(moduleConstants.driveEncoderA, moduleConstants.driveEncoderB); + driveEncoder.setDistancePerPulse(kDriveDistPerPulse); + steerMotor = new PWMSparkMax(moduleConstants.steerMotorID); + steerEncoder = new Encoder(moduleConstants.steerEncoderA, moduleConstants.steerEncoderB); + steerEncoder.setDistancePerPulse(kSteerRadPerPulse); + + steerPidController.enableContinuousInput(-Math.PI, Math.PI); + + // --- Simulation + driveEncoderSim = new EncoderSim(driveEncoder); + steerEncoderSim = new EncoderSim(steerEncoder); + } + + public void periodic() { + // Perform PID feedback control to steer the module to the target angle + double steerPid = + steerPidController.calculate( + getAbsoluteHeading().getRadians(), desiredState.angle.getRadians()); + steerMotor.setVoltage(steerPid); + + // Use feedforward model to translate target drive velocities into voltages + double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double drivePid = 0; + if (!openLoop) { + // Perform PID feedback control to compensate for disturbances + drivePid = + drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + } + + driveMotor.setVoltage(driveFF + drivePid); + } + + /** + * Command this swerve module to the desired angle and velocity. + * + * @param steerInPlace If modules should steer to target angle when target velocity is 0 + */ + public void setDesiredState( + SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) { + Rotation2d currentRotation = getAbsoluteHeading(); + // Avoid turning more than 90 degrees by inverting speed on large angle changes + desiredState = SwerveModuleState.optimize(desiredState, currentRotation); + + this.desiredState = desiredState; + } + + /** Module heading reported by steering encoder. */ + public Rotation2d getAbsoluteHeading() { + return Rotation2d.fromRadians(steerEncoder.getDistance()); + } + + /** + * {@link SwerveModuleState} describing absolute module rotation and velocity in meters per + * second. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(driveEncoder.getRate(), getAbsoluteHeading()); + } + + /** {@link SwerveModulePosition} describing absolute module rotation and position in meters. */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(driveEncoder.getDistance(), getAbsoluteHeading()); + } + + /** Voltage of the drive motor */ + public double getDriveVoltage() { + return driveMotor.get() * RobotController.getBatteryVoltage(); + } + + /** Voltage of the steer motor */ + public double getSteerVoltage() { + return steerMotor.get() * RobotController.getBatteryVoltage(); + } + + /** + * Constants about this module, like motor IDs, encoder angle offset, and translation from center. + */ + public ModuleConstants getModuleConstants() { + return moduleConstants; + } + + public void log() { + var state = getState(); + + String table = "Module " + moduleConstants.moduleNum + "/"; + SmartDashboard.putNumber( + table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); + SmartDashboard.putNumber( + table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber( + table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", + Units.metersToFeet(desiredState.speedMetersPerSecond)); + SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); + SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); + } + + // ----- Simulation + + public void simulationUpdate( + double driveEncoderDist, + double driveEncoderRate, + double driveCurrent, + double steerEncoderDist, + double steerEncoderRate, + double steerCurrent) { + driveEncoderSim.setDistance(driveEncoderDist); + driveEncoderSim.setRate(driveEncoderRate); + this.driveCurrentSim = driveCurrent; + steerEncoderSim.setDistance(steerEncoderDist); + steerEncoderSim.setRate(steerEncoderRate); + this.steerCurrentSim = steerCurrent; + } + + public double getDriveCurrentSim() { + return driveCurrentSim; + } + + public double getSteerCurrentSim() { + return steerCurrentSim; + } +} diff --git a/photonlib-java-examples/aimattarget/swerve_module.png b/photonlib-java-examples/aimattarget/swerve_module.png new file mode 100644 index 000000000..25990c839 Binary files /dev/null and b/photonlib-java-examples/aimattarget/swerve_module.png differ diff --git a/photonlib-java-examples/aimattarget/tag-blue.png b/photonlib-java-examples/aimattarget/tag-blue.png new file mode 100644 index 000000000..04b9e4f7b Binary files /dev/null and b/photonlib-java-examples/aimattarget/tag-blue.png differ diff --git a/photonlib-java-examples/aimattarget/tag-green.png b/photonlib-java-examples/aimattarget/tag-green.png new file mode 100644 index 000000000..63029fcf2 Binary files /dev/null and b/photonlib-java-examples/aimattarget/tag-green.png differ diff --git a/photonlib-java-examples/bin/main/org/photonlib/examples/examples.json b/photonlib-java-examples/bin/main/org/photonlib/examples/examples.json index 40c05cc5a..e148e99ce 100644 --- a/photonlib-java-examples/bin/main/org/photonlib/examples/examples.json +++ b/photonlib-java-examples/bin/main/org/photonlib/examples/examples.json @@ -11,18 +11,6 @@ "dependencies": [], "foldername": "aimattarget" }, - { - "name": "GetInRange", - "description": "Get in range of a target", - "tags": [], - "gradlebase": "java", - "language": "java", - "commandversion": 1, - "mainclass": "Main", - "packagetoreplace": null, - "dependencies": [], - "foldername": "getinrange" - }, { "name": "AimAndRange", "description": "Aim at a target while at a desired range", @@ -36,8 +24,8 @@ "foldername": "aimandrange" }, { - "name": "SimAimAndRange", - "description": "Adding Simulation Support to the Aim And Range example", + "name": "PoseEst", + "description": "Integrate 3D vision processing mode results into estimation of robot pose on the field.", "tags": [], "gradlebase": "java", "language": "java", @@ -45,18 +33,6 @@ "mainclass": "Main", "packagetoreplace": null, "dependencies": [], - "foldername": "simaimandrange" - }, - { - "name": "SimPoseEstimation", - "description": "Integrate 3D vision processing mode results into estimation of robot pose on the field. Includes simulation support.", - "tags": [], - "gradlebase": "java", - "language": "java", - "commandversion": 1, - "mainclass": "Main", - "packagetoreplace": null, - "dependencies": [], - "foldername": "simposeest" + "foldername": "poseest" } ] diff --git a/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/blue_square.png b/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/blue_square.png deleted file mode 100644 index 63b52bdd4..000000000 Binary files a/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/blue_square.png and /dev/null differ diff --git a/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/green_square.png b/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/green_square.png deleted file mode 100644 index d70576a43..000000000 Binary files a/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/green_square.png and /dev/null differ diff --git a/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/red_square.png b/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/red_square.png deleted file mode 100644 index 733814f18..000000000 Binary files a/photonlib-java-examples/bin/main/org/photonlib/examples/simposeest/resources/red_square.png and /dev/null differ diff --git a/photonlib-java-examples/getinrange/.wpilib/wpilib_preferences.json b/photonlib-java-examples/getinrange/.wpilib/wpilib_preferences.json deleted file mode 100644 index c72767ad4..000000000 --- a/photonlib-java-examples/getinrange/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2024", - "teamNumber": 6995 -} diff --git a/photonlib-java-examples/getinrange/README.md b/photonlib-java-examples/getinrange/README.md deleted file mode 100644 index 0366ed13a..000000000 --- a/photonlib-java-examples/getinrange/README.md +++ /dev/null @@ -1,3 +0,0 @@ -## **`getinrange`** - -### See [PhotonLib Java Examples](../README.md#getinrange) diff --git a/photonlib-java-examples/getinrange/WPILib-License.md b/photonlib-java-examples/getinrange/WPILib-License.md deleted file mode 100644 index 3d5a824ca..000000000 --- a/photonlib-java-examples/getinrange/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-java-examples/getinrange/build.gradle b/photonlib-java-examples/getinrange/build.gradle deleted file mode 100644 index 8c7ef1e20..000000000 --- a/photonlib-java-examples/getinrange/build.gradle +++ /dev/null @@ -1,98 +0,0 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" -} - -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 - -apply from: "${rootDir}/../shared/examples_common.gradle" - -def ROBOT_MAIN_CLASS = "frc.robot.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -// Set to true to use debug for JNI. -wpi.java.debugJni = false - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. -dependencies { - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() - - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() - - testImplementation 'junit:junit:4.13.1' -} - -// Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// knows where to look for our Robot Class. -jar { - from { - configurations.runtimeClasspath.collect { - it.isDirectory() ? it : zipTree(it) - } - } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE -} - -// Configure jar and deploy tasks -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) - -// Configure string concat to always inline compile -tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' -} diff --git a/photonlib-java-examples/getinrange/settings.gradle b/photonlib-java-examples/getinrange/settings.gradle deleted file mode 100644 index cfa00c4e1..000000000 --- a/photonlib-java-examples/getinrange/settings.gradle +++ /dev/null @@ -1,29 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -rootProject.name = 'getinrange' - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} diff --git a/photonlib-java-examples/getinrange/simgui.json b/photonlib-java-examples/getinrange/simgui.json deleted file mode 100644 index cc05c0f4c..000000000 --- a/photonlib-java-examples/getinrange/simgui.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" - } - }, - "NetworkTables": { - "transitory": { - "SmartDashboard": { - "Field": { - "open": true - }, - "open": true - } - } - } -} diff --git a/photonlib-java-examples/getinrange/src/main/java/frc/robot/Main.java b/photonlib-java-examples/getinrange/src/main/java/frc/robot/Main.java deleted file mode 100644 index 077f979f9..000000000 --- a/photonlib-java-examples/getinrange/src/main/java/frc/robot/Main.java +++ /dev/null @@ -1,45 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ -public final class Main { - private Main() {} - - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/photonlib-java-examples/getinrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/getinrange/src/main/java/frc/robot/Robot.java deleted file mode 100644 index c77602184..000000000 --- a/photonlib-java-examples/getinrange/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -1,107 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonUtils; - -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ -public class Robot extends TimedRobot { - // Constants such as camera and target height stored. Change per robot and goal! - final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); - final double TARGET_HEIGHT_METERS = Units.feetToMeters(5); - - // Angle between horizontal and the camera. - final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0); - - // How far from the target we want to be - final double GOAL_RANGE_METERS = Units.feetToMeters(3); - - // Change this to match the name of your camera - PhotonCamera camera = new PhotonCamera("photonvision"); - - // PID constants should be tuned per robot - final double P_GAIN = 0.1; - final double D_GAIN = 0.0; - PIDController controller = new PIDController(P_GAIN, 0, D_GAIN); - - XboxController xboxController; - - // Drive motors - PWMVictorSPX leftMotor = new PWMVictorSPX(0); - PWMVictorSPX rightMotor = new PWMVictorSPX(1); - DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); - - @Override - public void robotInit() { - xboxController = new XboxController(0); - } - - @Override - public void teleopPeriodic() { - double forwardSpeed; - double rotationSpeed = xboxController.getLeftX(); - - if (xboxController.getAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - var result = camera.getLatestResult(); - - if (result.hasTargets()) { - // First calculate range - double range = - PhotonUtils.calculateDistanceToTargetMeters( - CAMERA_HEIGHT_METERS, - TARGET_HEIGHT_METERS, - CAMERA_PITCH_RADIANS, - Units.degreesToRadians(result.getBestTarget().getPitch())); - - // Use this range as the measurement we give to the PID controller. - // -1.0 required to ensure positive PID controller effort _increases_ range - forwardSpeed = -controller.calculate(range, GOAL_RANGE_METERS); - } else { - // If we have no targets, stay still. - forwardSpeed = 0; - } - } else { - // Manual Driver Mode - forwardSpeed = -xboxController.getRightY(); - } - - // Use our forward/turn speeds to control the drivetrain - drive.arcadeDrive(forwardSpeed, rotationSpeed); - } -} diff --git a/photonlib-java-examples/swervedriveposeestsim/.gitignore b/photonlib-java-examples/poseest/.gitignore similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/.gitignore rename to photonlib-java-examples/poseest/.gitignore diff --git a/photonlib-java-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-java-examples/poseest/.wpilib/wpilib_preferences.json similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json rename to photonlib-java-examples/poseest/.wpilib/wpilib_preferences.json diff --git a/photonlib-java-examples/poseest/README.md b/photonlib-java-examples/poseest/README.md new file mode 100644 index 000000000..091309f47 --- /dev/null +++ b/photonlib-java-examples/poseest/README.md @@ -0,0 +1,3 @@ +## **`poseest`** + +### See [PhotonLib Java Examples](../README.md#poseest) diff --git a/photonlib-cpp-examples/getinrange/WPILib-License.md b/photonlib-java-examples/poseest/WPILib-License.md similarity index 96% rename from photonlib-cpp-examples/getinrange/WPILib-License.md rename to photonlib-java-examples/poseest/WPILib-License.md index 3d5a824ca..645e54253 100644 --- a/photonlib-cpp-examples/getinrange/WPILib-License.md +++ b/photonlib-java-examples/poseest/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/photonlib-java-examples/swervedriveposeestsim/build.gradle b/photonlib-java-examples/poseest/build.gradle similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/build.gradle rename to photonlib-java-examples/poseest/build.gradle diff --git a/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.jar b/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 000000000..249e5832f Binary files /dev/null and b/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.jar differ diff --git a/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 000000000..ca8f28abd --- /dev/null +++ b/photonlib-java-examples/poseest/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/photonlib-java-examples/swervedriveposeestsim/gradlew b/photonlib-java-examples/poseest/gradlew similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/gradlew rename to photonlib-java-examples/poseest/gradlew diff --git a/photonlib-cpp-examples/getinrange/gradlew.bat b/photonlib-java-examples/poseest/gradlew.bat similarity index 96% rename from photonlib-cpp-examples/getinrange/gradlew.bat rename to photonlib-java-examples/poseest/gradlew.bat index 53a6b238d..f127cfd49 100644 --- a/photonlib-cpp-examples/getinrange/gradlew.bat +++ b/photonlib-java-examples/poseest/gradlew.bat @@ -1,91 +1,91 @@ -@rem -@rem Copyright 2015 the original author or authors. -@rem -@rem Licensed under the Apache License, Version 2.0 (the "License"); -@rem you may not use this file except in compliance with the License. -@rem You may obtain a copy of the License at -@rem -@rem https://www.apache.org/licenses/LICENSE-2.0 -@rem -@rem Unless required by applicable law or agreed to in writing, software -@rem distributed under the License is distributed on an "AS IS" BASIS, -@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -@rem See the License for the specific language governing permissions and -@rem limitations under the License. -@rem - -@if "%DEBUG%"=="" @echo off -@rem ########################################################################## -@rem -@rem Gradle startup script for Windows -@rem -@rem ########################################################################## - -@rem Set local scope for the variables with windows NT shell -if "%OS%"=="Windows_NT" setlocal - -set DIRNAME=%~dp0 -if "%DIRNAME%"=="" set DIRNAME=. -set APP_BASE_NAME=%~n0 -set APP_HOME=%DIRNAME% - -@rem Resolve any "." and ".." in APP_HOME to make it shorter. -for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi - -@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" - -@rem Find java.exe -if defined JAVA_HOME goto findJavaFromJavaHome - -set JAVA_EXE=java.exe -%JAVA_EXE% -version >NUL 2>&1 -if %ERRORLEVEL% equ 0 goto execute - -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:findJavaFromJavaHome -set JAVA_HOME=%JAVA_HOME:"=% -set JAVA_EXE=%JAVA_HOME%/bin/java.exe - -if exist "%JAVA_EXE%" goto execute - -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. - -goto fail - -:execute -@rem Setup the command line - -set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar - - -@rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* - -:end -@rem End local scope for the variables with windows NT shell -if %ERRORLEVEL% equ 0 goto mainEnd - -:fail -rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of -rem the _cmd.exe /c_ return code! -set EXIT_CODE=%ERRORLEVEL% -if %EXIT_CODE% equ 0 set EXIT_CODE=1 -if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% -exit /b %EXIT_CODE% - -:mainEnd -if "%OS%"=="Windows_NT" endlocal - -:omega +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-java-examples/swervedriveposeestsim/settings.gradle b/photonlib-java-examples/poseest/settings.gradle similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/settings.gradle rename to photonlib-java-examples/poseest/settings.gradle diff --git a/photonlib-cpp-examples/getinrange/simgui-ds.json b/photonlib-java-examples/poseest/simgui-ds.json similarity index 94% rename from photonlib-cpp-examples/getinrange/simgui-ds.json rename to photonlib-java-examples/poseest/simgui-ds.json index bd35d5052..addd5860c 100644 --- a/photonlib-cpp-examples/getinrange/simgui-ds.json +++ b/photonlib-java-examples/poseest/simgui-ds.json @@ -11,10 +11,13 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], "axisCount": 6, diff --git a/photonlib-java-examples/swervedriveposeestsim/simgui.json b/photonlib-java-examples/poseest/simgui.json similarity index 64% rename from photonlib-java-examples/swervedriveposeestsim/simgui.json rename to photonlib-java-examples/poseest/simgui.json index 76fbf80d3..6bc1edd79 100644 --- a/photonlib-java-examples/swervedriveposeestsim/simgui.json +++ b/photonlib-java-examples/poseest/simgui.json @@ -62,7 +62,7 @@ "length": 0.6000000238418579, "width": 0.5 }, - "bottom": 544, + "bottom": 1476, "cameras": { "arrowColor": [ 0.29535865783691406, @@ -77,11 +77,11 @@ "weight": 1.0, "width": 1.0 }, - "height": 8.013679504394531, + "height": 8.210550308227539, "image": "2023-field.png", - "left": 46, - "right": 1088, - "top": 36, + "left": 150, + "right": 2961, + "top": 79, "visibleTargetPoses": { "arrows": false, "image": "tag-blue.png", @@ -98,6 +98,15 @@ }, "NetworkTables": { "transitory": { + "CameraPublisher": { + "YOUR CAMERA NAME-processed": { + "open": true, + "string[]##v_/CameraPublisher/YOUR CAMERA NAME-processed/streams": { + "open": true + } + }, + "open": true + }, "SmartDashboard": { "VisionSystemSim-main": { "open": true @@ -105,5 +114,48 @@ "open": true } } + }, + "NetworkTables Info": { + "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { +"axis": [ + { + "autoFit": true + } + ], + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)" + } + ] + } + ] + } } } diff --git a/photonlib-java-examples/getinrange/src/main/deploy/example.txt b/photonlib-java-examples/poseest/src/main/deploy/example.txt similarity index 100% rename from photonlib-java-examples/getinrange/src/main/deploy/example.txt rename to photonlib-java-examples/poseest/src/main/deploy/example.txt diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java rename to photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Main.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java similarity index 100% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Main.java rename to photonlib-java-examples/poseest/src/main/java/frc/robot/Main.java diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java similarity index 58% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java rename to photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java index d32d4d477..0425f8185 100644 --- a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java @@ -24,33 +24,24 @@ package frc.robot; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.subsystems.GamepieceLauncher; import frc.robot.subsystems.drivetrain.SwerveDrive; -import java.util.Random; public class Robot extends TimedRobot { private SwerveDrive drivetrain; private Vision vision; - private XboxController controller; - // Limit max speed - private final double kDriveSpeed = 0.6; - // Rudimentary limiting of drivetrain acceleration - private SlewRateLimiter forwardLimiter = new SlewRateLimiter(1.0 / 0.6); // 1 / x seconds to 100% - private SlewRateLimiter strafeLimiter = new SlewRateLimiter(1.0 / 0.6); - private SlewRateLimiter turnLimiter = new SlewRateLimiter(1.0 / 0.33); + private GamepieceLauncher gpLauncher; - private Timer autoTimer = new Timer(); - private Random rand = new Random(4512); + private XboxController controller; @Override public void robotInit() { @@ -58,31 +49,36 @@ public class Robot extends TimedRobot { vision = new Vision(); controller = new XboxController(0); + + gpLauncher = new GamepieceLauncher(); } @Override public void robotPeriodic() { + // Update Gamepiece Launcher Subsystem + gpLauncher.periodic(); + + // Update drivetrain subsystem drivetrain.periodic(); // Correct pose estimate with vision measurements var visionEst = vision.getEstimatedGlobalPose(); visionEst.ifPresent( est -> { - var estPose = est.estimatedPose.toPose2d(); // Change our trust in the measurement based on the tags we can see - var estStdDevs = vision.getEstimationStdDevs(estPose); + var estStdDevs = vision.getEstimationStdDevs(); drivetrain.addVisionMeasurement( est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); }); - // Apply a random offset to pose estimator to test vision correction + // Test/Example only! + // Apply an offset to pose estimator to test vision correction + // You probably don't want this on a real robot, just delete it. if (controller.getBButtonPressed()) { - var trf = - new Transform2d( - new Translation2d(rand.nextDouble() * 4 - 2, rand.nextDouble() * 4 - 2), - new Rotation2d(rand.nextDouble() * 2 * Math.PI)); - drivetrain.resetPose(drivetrain.getPose().plus(trf), false); + var disturbance = + new Transform2d(new Translation2d(1.0, 1.0), new Rotation2d(0.17 * 2 * Math.PI)); + drivetrain.resetPose(drivetrain.getPose().plus(disturbance), false); } // Log values to the dashboard @@ -95,45 +91,24 @@ public class Robot extends TimedRobot { } @Override - public void autonomousInit() { - autoTimer.restart(); - var pose = new Pose2d(1, 1, new Rotation2d()); - drivetrain.resetPose(pose, true); - vision.resetSimPose(pose); - } - - @Override - public void autonomousPeriodic() { - // translate diagonally while spinning - if (autoTimer.get() < 10) { - drivetrain.drive(0.5, 0.5, 0.5, false); - } else { - autoTimer.stop(); - drivetrain.stop(); - } + public void teleopInit() { + resetPose(); } @Override public void teleopPeriodic() { - // We will use an "arcade drive" scheme to turn joystick values into target robot speeds - // We want to get joystick values where pushing forward/left is positive - double forward = -controller.getLeftY() * kDriveSpeed; - if (Math.abs(forward) < 0.1) forward = 0; // deadband small values - forward = forwardLimiter.calculate(forward); // limit acceleration - double strafe = -controller.getLeftX() * kDriveSpeed; - if (Math.abs(strafe) < 0.1) strafe = 0; - strafe = strafeLimiter.calculate(strafe); - double turn = -controller.getRightX() * kDriveSpeed; - if (Math.abs(turn) < 0.1) turn = 0; - turn = turnLimiter.calculate(turn); - - // Convert from joystick values to real target speeds - forward *= Constants.Swerve.kMaxLinearSpeed; - strafe *= Constants.Swerve.kMaxLinearSpeed; - turn *= Constants.Swerve.kMaxLinearSpeed; + // Calculate drivetrain commands from Joystick values + double forward = -controller.getLeftY() * Constants.Swerve.kMaxLinearSpeed; + double strafe = -controller.getLeftX() * Constants.Swerve.kMaxLinearSpeed; + double turn = -controller.getRightX() * Constants.Swerve.kMaxAngularSpeed; // Command drivetrain motors based on target speeds - drivetrain.drive(forward, strafe, turn, true); + drivetrain.drive(forward, strafe, turn); + + // Calculate whether the gamepiece launcher runs based on our global pose estimate. + var curPose = drivetrain.getPose(); + var shouldRun = (curPose.getY() > 2.0 && curPose.getX() < 4.0); // Close enough to blue speaker + gpLauncher.setRunning(shouldRun); } @Override @@ -148,8 +123,20 @@ public class Robot extends TimedRobot { debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose()); debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); + // Update gamepiece launcher simulation + gpLauncher.simulationPeriodic(); + // Calculate battery voltage sag due to current draw RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); } + + public void resetPose() { + // Example Only - startPose should be derived from some assumption + // of where your robot was placed on the field. + // The first pose in an autonomous path is often a good choice. + var startPose = new Pose2d(1, 1, new Rotation2d()); + drivetrain.resetPose(startPose, true); + vision.resetSimPose(startPose); + } } diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java similarity index 65% rename from photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java rename to photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java index f156099cd..81e505394 100644 --- a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java @@ -33,6 +33,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -41,11 +42,12 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; public class Vision { private final PhotonCamera camera; private final PhotonPoseEstimator photonEstimator; + private Matrix curStdDevs; // Simulation private PhotonCameraSim cameraSim; @@ -81,14 +83,13 @@ public class Vision { } } - public PhotonPipelineResult getLatestResult() { - return camera.getLatestResult(); - } - /** * The latest estimated robot pose on the field from vision data. This may be empty. This should * only be called once per loop. * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets * used for estimation. */ @@ -96,6 +97,7 @@ public class Vision { Optional visionEst = Optional.empty(); for (var change : camera.getAllUnreadResults()) { visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); if (Robot.isSimulation()) { visionEst.ifPresentOrElse( @@ -112,34 +114,62 @@ public class Vision { } /** - * The standard deviations of the estimated pose from {@link #getEstimatedGlobalPose()}, for use - * with {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. - * This should only be used when there are targets visible. + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + * deviations based on number of tags, estimation strategy, and distance from the tags. * * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame */ - public Matrix getEstimationStdDevs(Pose2d estimatedPose) { - var estStdDevs = kSingleTagStdDevs; - var targets = getLatestResult().getTargets(); - int numTags = 0; - double avgDist = 0; - for (var tgt : targets) { - var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) continue; - numTags++; - avgDist += - tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); - } - if (numTags == 0) return estStdDevs; - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = kMultiTagStdDevs; - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = kSingleTagStdDevs; - return estStdDevs; + } else { + // Pose present. Start running Heuristic + var estStdDevs = kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; } // ----- Simulation diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java new file mode 100644 index 000000000..24634d616 --- /dev/null +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java @@ -0,0 +1,76 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class GamepieceLauncher { + private final PWMSparkMax motor; + + private final double LAUNCH_SPEED_RPM = 2500; + private double curDesSpd; + private double curMotorCmd = 0.0; + + public GamepieceLauncher() { + motor = new PWMSparkMax(8); + simulationInit(); + } + + public void setRunning(boolean shouldRun) { + curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0; + } + + public void periodic() { + double maxRPM = + Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec); + curMotorCmd = curDesSpd / maxRPM; + curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); + motor.set(curMotorCmd); + + SmartDashboard.putNumber("GPLauncher Des Spd (RPM)", curDesSpd); + } + + // -- SIMULATION SUPPORT + private DCMotor motorSim; + private FlywheelSim launcherSim; + + private void simulationInit() { + motorSim = DCMotor.getFalcon500(1); + launcherSim = new FlywheelSim(motorSim, 1.0, 0.002); + } + + public void simulationPeriodic() { + launcherSim.setInputVoltage(curMotorCmd * RobotController.getBatteryVoltage()); + launcherSim.update(0.02); + var spd = launcherSim.getAngularVelocityRPM(); + SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd); + } +} diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java new file mode 100644 index 000000000..b41017a3e --- /dev/null +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -0,0 +1,332 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.SPI.Port; +import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class SwerveDrive { + // Construct the swerve modules with their respective constants. + // The SwerveModule class will handle all the details of controlling the modules. + private final SwerveModule[] swerveMods = { + new SwerveModule(ModuleConstants.FL), + new SwerveModule(ModuleConstants.FR), + new SwerveModule(ModuleConstants.BL), + new SwerveModule(ModuleConstants.BR) + }; + + // The kinematics for translating between robot chassis speeds and module states. + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + swerveMods[0].getModuleConstants().centerOffset, + swerveMods[1].getModuleConstants().centerOffset, + swerveMods[2].getModuleConstants().centerOffset, + swerveMods[3].getModuleConstants().centerOffset); + + private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0); + + // The robot pose estimator for tracking swerve odometry and applying vision corrections. + private final SwerveDrivePoseEstimator poseEstimator; + + private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds(); + + // ----- Simulation + private final ADXRS450_GyroSim gyroSim; + private final SwerveDriveSim swerveDriveSim; + private double totalCurrentDraw = 0; + + public SwerveDrive() { + // Define the standard deviations for the pose estimator, which determine how fast the pose + // estimate converges to the vision measurement. This should depend on the vision measurement + // noise + // and how many or how frequently vision measurements are applied to the pose estimator. + var stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + var visionStdDevs = VecBuilder.fill(1, 1, 1); + poseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getGyroYaw(), + getModulePositions(), + new Pose2d(), + stateStdDevs, + visionStdDevs); + + // ----- Simulation + gyroSim = new ADXRS450_GyroSim(gyro); + swerveDriveSim = + new SwerveDriveSim( + kDriveFF, + DCMotor.getFalcon500(1), + kDriveGearRatio, + kWheelDiameter / 2.0, + kSteerFF, + DCMotor.getFalcon500(1), + kSteerGearRatio, + kinematics); + } + + public void periodic() { + for (SwerveModule module : swerveMods) { + module.periodic(); + } + + // Update the odometry of the swerve drive using the wheel encoders and gyro. + poseEstimator.update(getGyroYaw(), getModulePositions()); + } + + /** + * Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to + * specific swerve module states. + * + * @param vxMeters X velocity (forwards/backwards) + * @param vyMeters Y velocity (strafe left/right) + * @param omegaRadians Angular velocity (rotation CCW+) + */ + public void drive(double vxMeters, double vyMeters, double omegaRadians) { + var targetChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading()); + setChassisSpeeds(targetChassisSpeeds, true, false); + } + + /** + * Command the swerve drive to the desired chassis speeds by converting them to swerve module + * states and using {@link #setModuleStates(SwerveModuleState[], boolean)}. + * + * @param targetChassisSpeeds Target robot-relative chassis speeds (vx, vy, omega). + * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback + * control. + * @param steerInPlace If modules should steer to the target angle when target velocity is 0. + */ + public void setChassisSpeeds( + ChassisSpeeds targetChassisSpeeds, boolean openLoop, boolean steerInPlace) { + setModuleStates(kinematics.toSwerveModuleStates(targetChassisSpeeds), openLoop, steerInPlace); + this.targetChassisSpeeds = targetChassisSpeeds; + } + + /** + * Command the swerve modules to the desired states. Velocites exceeding the maximum speed will be + * desaturated (while preserving the ratios between modules). + * + * @param openLoop If swerve modules should use feedforward only and ignore velocity feedback + * control. + * @param steerInPlace If modules should steer to the target angle when target velocity is 0. + */ + public void setModuleStates( + SwerveModuleState[] desiredStates, boolean openLoop, boolean steerInPlace) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, kMaxLinearSpeed); + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].setDesiredState(desiredStates[i], openLoop, steerInPlace); + } + } + + /** Stop the swerve drive. */ + public void stop() { + drive(0, 0, 0); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */ + public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds); + } + + /** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */ + public void addVisionMeasurement( + Pose2d visionMeasurement, double timestampSeconds, Matrix stdDevs) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); + } + + /** + * Reset the estimated pose of the swerve drive on the field. + * + * @param pose New robot pose. + * @param resetSimPose If the simulated robot pose should also be reset. This effectively + * teleports the robot and should only be used during the setup of the simulation world. + */ + public void resetPose(Pose2d pose, boolean resetSimPose) { + if (resetSimPose) { + swerveDriveSim.reset(pose, false); + // we shouldnt realistically be resetting pose after startup, but we will handle it anyway for + // testing + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); + } + gyroSim.setAngle(-pose.getRotation().getDegrees()); + gyroSim.setRate(0); + } + + poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + /** Get the estimated pose of the swerve drive on the field. */ + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** The heading of the swerve drive's estimated pose on the field. */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** Raw gyro yaw (this may not match the field heading!). */ + public Rotation2d getGyroYaw() { + return gyro.getRotation2d(); + } + + /** Get the chassis speeds of the robot (vx, vy, omega) from the swerve module states. */ + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** + * Get the SwerveModuleState of each swerve module (speed, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + swerveMods[0].getState(), + swerveMods[1].getState(), + swerveMods[2].getState(), + swerveMods[3].getState() + }; + } + + /** + * Get the SwerveModulePosition of each swerve module (position, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + swerveMods[0].getPosition(), + swerveMods[1].getPosition(), + swerveMods[2].getPosition(), + swerveMods[3].getPosition() + }; + } + + /** + * Get the Pose2d of each swerve module based on kinematics and current robot pose. The returned + * array order matches the kinematics module order. + */ + public Pose2d[] getModulePoses() { + Pose2d[] modulePoses = new Pose2d[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + var module = swerveMods[i]; + modulePoses[i] = + getPose() + .transformBy( + new Transform2d( + module.getModuleConstants().centerOffset, module.getAbsoluteHeading())); + } + return modulePoses; + } + + /** Log various drivetrain values to the dashboard. */ + public void log() { + String table = "Drive/"; + Pose2d pose = getPose(); + SmartDashboard.putNumber(table + "X", pose.getX()); + SmartDashboard.putNumber(table + "Y", pose.getY()); + SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + + for (SwerveModule module : swerveMods) { + module.log(); + } + } + + // ----- Simulation + + public void simulationPeriodic() { + // Pass commanded motor voltages into swerve drive simulation + double[] driveInputs = new double[swerveMods.length]; + double[] steerInputs = new double[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + driveInputs[i] = swerveMods[i].getDriveVoltage(); + steerInputs[i] = swerveMods[i].getSteerVoltage(); + } + swerveDriveSim.setDriveInputs(driveInputs); + swerveDriveSim.setSteerInputs(steerInputs); + + // Simulate one timestep + swerveDriveSim.update(Robot.kDefaultPeriod); + + // Update module and gyro values with simulated values + var driveStates = swerveDriveSim.getDriveStates(); + var steerStates = swerveDriveSim.getSteerStates(); + totalCurrentDraw = 0; + double[] driveCurrents = swerveDriveSim.getDriveCurrentDraw(); + for (double current : driveCurrents) totalCurrentDraw += current; + double[] steerCurrents = swerveDriveSim.getSteerCurrentDraw(); + for (double current : steerCurrents) totalCurrentDraw += current; + for (int i = 0; i < swerveMods.length; i++) { + double drivePos = driveStates.get(i).get(0, 0); + double driveRate = driveStates.get(i).get(1, 0); + double steerPos = steerStates.get(i).get(0, 0); + double steerRate = steerStates.get(i).get(1, 0); + swerveMods[i].simulationUpdate( + drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); + } + + gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + } + + /** + * The "actual" pose of the robot on the field used in simulation. This is different from the + * swerve drive's estimated pose. + */ + public Pose2d getSimPose() { + return swerveDriveSim.getPose(); + } + + public double getCurrentDraw() { + return totalCurrentDraw; + } +} diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java new file mode 100644 index 000000000..85849141d --- /dev/null +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -0,0 +1,495 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +/** + * This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users + * should first set inputs from motors with {@link #setDriveInputs(double...)} and {@link + * #setSteerInputs(double...)}, call {@link #update(double)} to update the simulation, and then set + * swerve module's encoder values and the drivetrain's gyro values with the results from this class. + * + *

In this class, distances are expressed with meters, angles with radians, and inputs with + * voltages. + * + *

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on + * the Sim GUI's field. + */ +public class SwerveDriveSim { + private final LinearSystem drivePlant; + private final double driveKs; + private final DCMotor driveMotor; + private final double driveGearing; + private final double driveWheelRadius; + private final LinearSystem steerPlant; + private final double steerKs; + private final DCMotor steerMotor; + private final double steerGearing; + + private final SwerveDriveKinematics kinematics; + private final int numModules; + + private final double[] driveInputs; + private final List> driveStates; + private final double[] steerInputs; + private final List> steerStates; + + private final Random rand = new Random(); + + // noiseless "actual" pose of the robot on the field + private Pose2d pose = new Pose2d(); + private double omegaRadsPerSec = 0; + + /** + * Creates a swerve drive simulation. + * + * @param driveFF The feedforward for the drive motors of this swerve drive. This should be in + * units of meters. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerFF The feedforward for the steer motors of this swerve drive. This should be in + * units of radians. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + SimpleMotorFeedforward driveFF, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + SimpleMotorFeedforward steerFF, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this( + new LinearSystem( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka), + VecBuilder.fill(0.0, 1.0 / driveFF.ka), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + VecBuilder.fill(0.0, 0.0)), + driveFF.ks, + driveMotor, + driveGearing, + driveWheelRadius, + new LinearSystem( + MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka), + VecBuilder.fill(0.0, 1.0 / steerFF.ka), + MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), + VecBuilder.fill(0.0, 0.0)), + steerFF.ks, + steerMotor, + steerGearing, + kinematics); + } + + /** + * Creates a swerve drive simulation. + * + * @param drivePlant The {@link LinearSystem} representing a swerve module's drive system. The + * state should be in units of meters and input in volts. + * @param driveKs The static gain in volts of the drive system's feedforward, or the minimum + * voltage to cause motion. Set this to 0 to ignore static friction. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerPlant The {@link LinearSystem} representing a swerve module's steer system. The + * state should be in units of radians and input in volts. + * @param steerKs The static gain in volts of the steer system's feedforward, or the minimum + * voltage to cause motion. Set this to 0 to ignore static friction. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + LinearSystem drivePlant, + double driveKs, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + LinearSystem steerPlant, + double steerKs, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this.drivePlant = drivePlant; + this.driveKs = driveKs; + this.driveMotor = driveMotor; + this.driveGearing = driveGearing; + this.driveWheelRadius = driveWheelRadius; + this.steerPlant = steerPlant; + this.steerKs = steerKs; + this.steerMotor = steerMotor; + this.steerGearing = steerGearing; + + this.kinematics = kinematics; + numModules = kinematics.toSwerveModuleStates(new ChassisSpeeds()).length; + driveInputs = new double[numModules]; + driveStates = new ArrayList<>(numModules); + steerInputs = new double[numModules]; + steerStates = new ArrayList<>(numModules); + for (int i = 0; i < numModules; i++) { + driveStates.add(VecBuilder.fill(0, 0)); + steerStates.add(VecBuilder.fill(0, 0)); + } + } + + /** + * Sets the input voltages of the drive motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setDriveInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < driveInputs.length; i++) { + double input = inputs[i]; + driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Sets the input voltages of the steer motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setSteerInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < steerInputs.length; i++) { + double input = inputs[i]; + steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Computes the new x given the old x and the control input. Includes the effect of static + * friction. + * + * @param discA The discretized system matrix. + * @param discB The discretized input matrix. + * @param x The position/velocity state of the drive/steer system. + * @param input The input voltage. + * @param ks The kS value of the feedforward model. + * @return The updated x, including the effect of static friction. + */ + protected static Matrix calculateX( + Matrix discA, Matrix discB, Matrix x, double input, double ks) { + var Ax = discA.times(x); + double nextStateVel = Ax.get(1, 0); + // input required to make next state vel == 0 + double inputToStop = nextStateVel / -discB.get(1, 0); + // ks effect on system velocity + double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + + // after ks system effect: + nextStateVel += discB.get(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB.get(1, 0); + double signToStop = Math.signum(inputToStop); + double inputSign = Math.signum(input); + double ksInputEffect = 0; + + // system velocity was reduced to 0, resist any input + if (Math.abs(ksSystemEffect) < ks) { + double absInput = Math.abs(input); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + // non-zero system velocity, but input causes velocity sign change. Resist input after sign + // change + else if ((input * signToStop) > (inputToStop * signToStop)) { + double absInput = Math.abs(input - inputToStop); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + + // calculate next x including static friction + var Bu = discB.times(VecBuilder.fill(input + ksSystemEffect + ksInputEffect)); + return Ax.plus(Bu); + } + + /** + * Update the drivetrain states with the given timestep. + * + * @param dtSeconds The timestep in seconds. This should be the robot loop period. + */ + public void update(double dtSeconds) { + var driveDiscAB = Discretization.discretizeAB(drivePlant.getA(), drivePlant.getB(), dtSeconds); + var steerDiscAB = Discretization.discretizeAB(steerPlant.getA(), steerPlant.getB(), dtSeconds); + + var moduleDeltas = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates.get(i).get(0, 0); + driveStates.set( + i, + calculateX( + driveDiscAB.getFirst(), + driveDiscAB.getSecond(), + driveStates.get(i), + driveInputs[i], + driveKs)); + double currDriveStatePos = driveStates.get(i).get(0, 0); + steerStates.set( + i, + calculateX( + steerDiscAB.getFirst(), + steerDiscAB.getSecond(), + steerStates.get(i), + steerInputs[i], + steerKs)); + double currSteerStatePos = steerStates.get(i).get(0, 0); + moduleDeltas[i] = + new SwerveModulePosition( + currDriveStatePos - prevDriveStatePos, new Rotation2d(currSteerStatePos)); + } + + var twist = kinematics.toTwist2d(moduleDeltas); + pose = pose.exp(twist); + omegaRadsPerSec = twist.dtheta / dtSeconds; + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param preserveMotion If true, the current module states will be preserved. Otherwise, they + * will be reset to zeros. + */ + public void reset(Pose2d pose, boolean preserveMotion) { + this.pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates.set(i, VecBuilder.fill(0, 0)); + steerStates.set(i, VecBuilder.fill(0, 0)); + } + omegaRadsPerSec = 0; + } + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param moduleDriveStates The new states of the modules' drive systems in [meters, meters/sec]. + * These should match the module order used in the kinematics. + * @param moduleSteerStates The new states of the modules' steer systems in [radians, + * radians/sec]. These should match the module order used in the kinematics. + */ + public void reset( + Pose2d pose, List> moduleDriveStates, List> moduleSteerStates) { + if (moduleDriveStates.size() != driveStates.size() + || moduleSteerStates.size() != steerStates.size()) + throw new IllegalArgumentException("Provided module states do not match number of modules!"); + this.pose = pose; + for (int i = 0; i < numModules; i++) { + driveStates.set(i, moduleDriveStates.get(i).copy()); + steerStates.set(i, moduleSteerStates.get(i).copy()); + } + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + } + + /** + * Get the pose of the simulated swerve drive. Note that this is the "actual" pose of the robot in + * the simulation world, without any noise. If you are simulating a pose estimator, this pose + * should only be used for visualization or camera simulation. This should be called after {@link + * #update(double)}. + */ + public Pose2d getPose() { + return pose; + } + + /** + * Get the {@link SwerveModulePosition} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModulePosition[] getModulePositions() { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the {@link SwerveModulePosition} of each module with rudimentary noise simulation. The + * returned array order matches the kinematics module order. This should be called after {@link + * #update(double)}. + * + * @param driveStdDev The standard deviation in meters of the drive wheel position. + * @param steerStdDev The standard deviation in radians of the steer angle. + */ + public SwerveModulePosition[] getNoisyModulePositions(double driveStdDev, double steerStdDev) { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0) + rand.nextGaussian() * driveStdDev, + new Rotation2d(steerStates.get(i).get(0, 0) + rand.nextGaussian() * steerStdDev)); + } + return positions; + } + + /** + * Get the {@link SwerveModuleState} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModuleState[] getModuleStates() { + var positions = new SwerveModuleState[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModuleState( + driveStates.get(i).get(1, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the state of each module's drive system in [meters, meters/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getDriveStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < driveStates.size(); i++) { + states.add(driveStates.get(i).copy()); + } + return states; + } + + /** + * Get the state of each module's steer system in [radians, radians/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getSteerStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < steerStates.size(); i++) { + states.add(steerStates.get(i).copy()); + } + return states; + } + + /** + * Get the angular velocity of the robot, which can be useful for gyro simulation. CCW positive. + * This should be called after {@link #update(double)}. + */ + public double getOmegaRadsPerSec() { + return omegaRadsPerSec; + } + + /** + * Calculates the current drawn from the battery by the motor(s). Ignores regenerative current + * from back-emf. + * + * @param motor The motor(s) used. + * @param radiansPerSec The velocity of the motor in radians per second. + * @param inputVolts The voltage commanded by the motor controller (battery voltage * duty cycle). + * @param battVolts The voltage of the battery. + */ + protected static double getCurrentDraw( + DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { + double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + // ignore regeneration + if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); + else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + // calculate battery current + return (inputVolts / battVolts) * (effVolts / motor.rOhms); + } + + /** + * Get the current draw in amps for each module's drive motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getDriveCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = driveStates.get(i).get(1, 0) * driveGearing / driveWheelRadius; + currents[i] = + getCurrentDraw( + driveMotor, radiansPerSec, driveInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the current draw in amps for each module's steer motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getSteerCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = steerStates.get(i).get(1, 0) * steerGearing; + currents[i] = + getCurrentDraw( + steerMotor, radiansPerSec, steerInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the total current draw in amps of all swerve motors. This should be called after {@link + * #update(double)}. + */ + public double getTotalCurrentDraw() { + double sum = 0; + for (double val : getDriveCurrentDraw()) sum += val; + for (double val : getSteerCurrentDraw()) sum += val; + return sum; + } +} diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java new file mode 100644 index 000000000..4615d2921 --- /dev/null +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -0,0 +1,192 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + // --- Module Constants + private final ModuleConstants moduleConstants; + + // --- Hardware + private final PWMSparkMax driveMotor; + private final Encoder driveEncoder; + private final PWMSparkMax steerMotor; + private final Encoder steerEncoder; + + // --- Control + private SwerveModuleState desiredState = new SwerveModuleState(); + private boolean openLoop = false; + + // Simple PID feedback controllers run on the roborio + private PIDController drivePidController = new PIDController(kDriveKP, kDriveKI, kDriveKD); + // (A profiled steering PID controller may give better results by utilizing feedforward.) + private PIDController steerPidController = new PIDController(kSteerKP, kSteerKI, kSteerKD); + + // --- Simulation + private final EncoderSim driveEncoderSim; + private double driveCurrentSim = 0; + private final EncoderSim steerEncoderSim; + private double steerCurrentSim = 0; + + public SwerveModule(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + driveMotor = new PWMSparkMax(moduleConstants.driveMotorID); + driveEncoder = new Encoder(moduleConstants.driveEncoderA, moduleConstants.driveEncoderB); + driveEncoder.setDistancePerPulse(kDriveDistPerPulse); + steerMotor = new PWMSparkMax(moduleConstants.steerMotorID); + steerEncoder = new Encoder(moduleConstants.steerEncoderA, moduleConstants.steerEncoderB); + steerEncoder.setDistancePerPulse(kSteerRadPerPulse); + + steerPidController.enableContinuousInput(-Math.PI, Math.PI); + + // --- Simulation + driveEncoderSim = new EncoderSim(driveEncoder); + steerEncoderSim = new EncoderSim(steerEncoder); + } + + public void periodic() { + // Perform PID feedback control to steer the module to the target angle + double steerPid = + steerPidController.calculate( + getAbsoluteHeading().getRadians(), desiredState.angle.getRadians()); + steerMotor.setVoltage(steerPid); + + // Use feedforward model to translate target drive velocities into voltages + double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double drivePid = 0; + if (!openLoop) { + // Perform PID feedback control to compensate for disturbances + drivePid = + drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + } + + driveMotor.setVoltage(driveFF + drivePid); + } + + /** + * Command this swerve module to the desired angle and velocity. + * + * @param steerInPlace If modules should steer to target angle when target velocity is 0 + */ + public void setDesiredState( + SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) { + Rotation2d currentRotation = getAbsoluteHeading(); + // Avoid turning more than 90 degrees by inverting speed on large angle changes + desiredState = SwerveModuleState.optimize(desiredState, currentRotation); + + this.desiredState = desiredState; + } + + /** Module heading reported by steering encoder. */ + public Rotation2d getAbsoluteHeading() { + return Rotation2d.fromRadians(steerEncoder.getDistance()); + } + + /** + * {@link SwerveModuleState} describing absolute module rotation and velocity in meters per + * second. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(driveEncoder.getRate(), getAbsoluteHeading()); + } + + /** {@link SwerveModulePosition} describing absolute module rotation and position in meters. */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(driveEncoder.getDistance(), getAbsoluteHeading()); + } + + /** Voltage of the drive motor */ + public double getDriveVoltage() { + return driveMotor.get() * RobotController.getBatteryVoltage(); + } + + /** Voltage of the steer motor */ + public double getSteerVoltage() { + return steerMotor.get() * RobotController.getBatteryVoltage(); + } + + /** + * Constants about this module, like motor IDs, encoder angle offset, and translation from center. + */ + public ModuleConstants getModuleConstants() { + return moduleConstants; + } + + public void log() { + var state = getState(); + + String table = "Module " + moduleConstants.moduleNum + "/"; + SmartDashboard.putNumber( + table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); + SmartDashboard.putNumber( + table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber( + table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", + Units.metersToFeet(desiredState.speedMetersPerSecond)); + SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); + SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); + } + + // ----- Simulation + + public void simulationUpdate( + double driveEncoderDist, + double driveEncoderRate, + double driveCurrent, + double steerEncoderDist, + double steerEncoderRate, + double steerCurrent) { + driveEncoderSim.setDistance(driveEncoderDist); + driveEncoderSim.setRate(driveEncoderRate); + this.driveCurrentSim = driveCurrent; + steerEncoderSim.setDistance(steerEncoderDist); + steerEncoderSim.setRate(steerEncoderRate); + this.steerCurrentSim = steerCurrent; + } + + public double getDriveCurrentSim() { + return driveCurrentSim; + } + + public double getSteerCurrentSim() { + return steerCurrentSim; + } +} diff --git a/photonlib-java-examples/poseest/swerve_module.png b/photonlib-java-examples/poseest/swerve_module.png new file mode 100644 index 000000000..25990c839 Binary files /dev/null and b/photonlib-java-examples/poseest/swerve_module.png differ diff --git a/photonlib-java-examples/poseest/tag-blue.png b/photonlib-java-examples/poseest/tag-blue.png new file mode 100644 index 000000000..04b9e4f7b Binary files /dev/null and b/photonlib-java-examples/poseest/tag-blue.png differ diff --git a/photonlib-java-examples/poseest/tag-green.png b/photonlib-java-examples/poseest/tag-green.png new file mode 100644 index 000000000..63029fcf2 Binary files /dev/null and b/photonlib-java-examples/poseest/tag-green.png differ diff --git a/photonlib-java-examples/simaimandrange/.wpilib/wpilib_preferences.json b/photonlib-java-examples/simaimandrange/.wpilib/wpilib_preferences.json deleted file mode 100644 index c72767ad4..000000000 --- a/photonlib-java-examples/simaimandrange/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2024", - "teamNumber": 6995 -} diff --git a/photonlib-java-examples/simaimandrange/2020-field.png b/photonlib-java-examples/simaimandrange/2020-field.png deleted file mode 100644 index 8b18648e8..000000000 Binary files a/photonlib-java-examples/simaimandrange/2020-field.png and /dev/null differ diff --git a/photonlib-java-examples/simaimandrange/2020-infiniterecharge.json b/photonlib-java-examples/simaimandrange/2020-infiniterecharge.json deleted file mode 100644 index 7109f5c6e..000000000 --- a/photonlib-java-examples/simaimandrange/2020-infiniterecharge.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "game": "Infinite Recharge", - "field-image": "2020-field.png", - "field-corners": { - "top-left": [ - 96, - 25 - ], - "bottom-right": [ - 1040, - 514 - ] - }, - "field-size": [ - 52.4375, - 26.9375 - ], - "field-unit": "foot" -} diff --git a/photonlib-java-examples/simaimandrange/README.md b/photonlib-java-examples/simaimandrange/README.md deleted file mode 100644 index 473f65079..000000000 --- a/photonlib-java-examples/simaimandrange/README.md +++ /dev/null @@ -1,3 +0,0 @@ -## **`simaimandrange`** - -### See [PhotonLib Java Examples](../README.md#simaimandrange) diff --git a/photonlib-java-examples/simaimandrange/WPILib-License.md b/photonlib-java-examples/simaimandrange/WPILib-License.md deleted file mode 100644 index 3d5a824ca..000000000 --- a/photonlib-java-examples/simaimandrange/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-java-examples/simaimandrange/build.gradle b/photonlib-java-examples/simaimandrange/build.gradle deleted file mode 100644 index 8c7ef1e20..000000000 --- a/photonlib-java-examples/simaimandrange/build.gradle +++ /dev/null @@ -1,98 +0,0 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" -} - -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 - -apply from: "${rootDir}/../shared/examples_common.gradle" - -def ROBOT_MAIN_CLASS = "frc.robot.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -// Set to true to use debug for JNI. -wpi.java.debugJni = false - -// Set this to true to enable desktop support. -def includeDesktopSupport = true - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. -dependencies { - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() - - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() - - testImplementation 'junit:junit:4.13.1' -} - -// Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// knows where to look for our Robot Class. -jar { - from { - configurations.runtimeClasspath.collect { - it.isDirectory() ? it : zipTree(it) - } - } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE -} - -// Configure jar and deploy tasks -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) - -// Configure string concat to always inline compile -tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' -} diff --git a/photonlib-java-examples/simaimandrange/settings.gradle b/photonlib-java-examples/simaimandrange/settings.gradle deleted file mode 100644 index 464d86e81..000000000 --- a/photonlib-java-examples/simaimandrange/settings.gradle +++ /dev/null @@ -1,29 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -rootProject.name = 'simaimandrange' - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2024' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} diff --git a/photonlib-java-examples/simaimandrange/simgui-ds.json b/photonlib-java-examples/simaimandrange/simgui-ds.json deleted file mode 100644 index 8dc94bc6a..000000000 --- a/photonlib-java-examples/simaimandrange/simgui-ds.json +++ /dev/null @@ -1,102 +0,0 @@ -{ - "keyboardJoysticks": [ - { - "axisConfig": [ - {}, - { - "decKey": 87, - "incKey": 83 - }, - { - "decayRate": 0.10000000149011612, - "incKey": 81, - "keyRate": 0.10000000149011612 - }, - { - "decayRate": 0.10000000149011612, - "incKey": 69, - "keyRate": 0.10000000149011612 - }, - { - "decKey": 65, - "incKey": 68 - } - ], - "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-java-examples/simaimandrange/simgui.json b/photonlib-java-examples/simaimandrange/simgui.json deleted file mode 100644 index 4ff3cc0f5..000000000 --- a/photonlib-java-examples/simaimandrange/simgui.json +++ /dev/null @@ -1,83 +0,0 @@ -{ - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" - }, - "windows": { - "/SmartDashboard/VisionSystemSim-main/Sim Field": { - "Robot": { - "arrowColor": [ - 1.0, - 1.0, - 1.0, - 255.0 - ], - "arrowWeight": 3.0, - "color": [ - 1.0, - 1.0, - 1.0, - 255.0 - ], - "weight": 3.0 - }, - "bottom": 514, - "cameras": { - "arrowColor": [ - 0.0, - 0.683544397354126, - 1.0, - 255.0 - ], - "arrowSize": 29, - "selectable": false, - "style": "Hidden" - }, - "height": 8.210550308227539, - "image": "2020-field.png", - "left": 96, - "right": 1040, - "targets": { - "arrowColor": [ - 0.27848100662231445, - 1.0, - 0.0, - 255.0 - ], - "arrowSize": 58, - "arrowWeight": 6.0, - "color": [ - 0.05063295364379883, - 1.0, - 0.0, - 255.0 - ], - "style": "Hidden", - "weight": 1.0 - }, - "top": 25, - "width": 15.982950210571289, - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "transitory": { - "SmartDashboard": { - "VisionSystemSim-main/Sim Field": { - "open": true - }, - "open": true - }, - "photonvision": { - "open": true, - "photonvision": { - "open": true - } - } - } - } -} diff --git a/photonlib-java-examples/simaimandrange/src/main/deploy/example.txt b/photonlib-java-examples/simaimandrange/src/main/deploy/example.txt deleted file mode 100644 index d6ec5cf83..000000000 --- a/photonlib-java-examples/simaimandrange/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java deleted file mode 100644 index 8e0f41cd5..000000000 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,91 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; - -public class Constants { - // ---------- Vision - // Constants about how your camera is mounted to the robot - public static final double CAMERA_PITCH_RADIANS = - Units.degreesToRadians(15); // Angle "up" from horizontal - public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor - - // How far from the target we want to be - public static final double GOAL_RANGE_METERS = Units.feetToMeters(10); - - // Where the 2020 High goal target is located on the field - // See - // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system - // and https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf - // (pages 4 and 5) - public static final Pose3d TARGET_POSE = - new Pose3d( - new Translation3d( - Units.feetToMeters(52.46), - Units.inchesToMeters(94.66), - Units.inchesToMeters(89.69)), // (center of vision target) - new Rotation3d(0.0, 0.0, Math.PI)); - // ---------- - - // ---------- Drivetrain - public static final int LEFT_MOTOR_CHANNEL = 0; - public static final int RIGHT_MOTOR_CHANNEL = 1; - - // PID constants should be tuned per robot - public static final double LINEAR_P = 0.5; - public static final double LINEAR_I = 0; - public static final double LINEAR_D = 0.1; - - public static final double ANGULAR_P = 0.03; - public static final double ANGULAR_I = 0; - public static final double ANGULAR_D = 0.003; - - // Ratio to multiply joystick inputs by - public static final double DRIVESPEED = 0.75; - - // The following properties are necessary for simulation: - - // Distance from drivetrain left wheels to right wheels - public static final double TRACKWIDTH_METERS = Units.feetToMeters(2.0); - public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(6.0); - - // The motors used in the gearbox for one drivetrain side - public static final DCMotor DRIVE_MOTORS = DCMotor.getCIM(2); - // The gearbox reduction, or how many motor rotations per wheel rotation - public static final double GEARING = 8.0; - - // The drivetrain feedforward values - public static final double LINEAR_KV = 2.0; - public static final double LINEAR_KA = 0.5; - - public static final double ANGULAR_KV = 2.25; - public static final double ANGULAR_KA = 0.3; - // ---------- -} diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Main.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Main.java deleted file mode 100644 index 077f979f9..000000000 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Main.java +++ /dev/null @@ -1,45 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ -public final class Main { - private Main() {} - - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java deleted file mode 100644 index 4f22aa287..000000000 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -1,125 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot; - -import static frc.robot.Constants.*; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; -import frc.robot.sim.DrivetrainSim; -import frc.robot.sim.VisionSim; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonUtils; - -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ -public class Robot extends TimedRobot { - // Change this to match the name of your camera - PhotonCamera camera = new PhotonCamera("photonvision"); - - // PID constants should be tuned per robot - PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); - PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); - - XboxController xboxController = new XboxController(0); - - // Drive motors - PWMVictorSPX leftMotor = new PWMVictorSPX(LEFT_MOTOR_CHANNEL); - PWMVictorSPX rightMotor = new PWMVictorSPX(RIGHT_MOTOR_CHANNEL); - DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); - - @Override - public void robotInit() { - leftMotor.setInverted(false); - rightMotor.setInverted(true); - } - - @Override - public void teleopPeriodic() { - double forwardSpeed; - double rotationSpeed; - - if (xboxController.getAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - var result = camera.getLatestResult(); - - if (result.hasTargets()) { - // First calculate range - double range = - PhotonUtils.calculateDistanceToTargetMeters( - CAMERA_HEIGHT_METERS, - TARGET_POSE.getZ(), - CAMERA_PITCH_RADIANS, - Units.degreesToRadians(result.getBestTarget().getPitch())); - - // Use this range as the measurement we give to the PID controller. - // (This forwardSpeed must be positive to go forward.) - forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS); - - // Also calculate angular power - // (This rotationSpeed must be positive to turn counter-clockwise.) - rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0); - } else { - // If we have no targets, stay still. - forwardSpeed = 0; - rotationSpeed = 0; - } - } else { - // Manual Driver Mode - forwardSpeed = -xboxController.getLeftY() * DRIVESPEED; - rotationSpeed = -xboxController.getRightX() * DRIVESPEED; - } - - // Use our forward/turn speeds to control the drivetrain - drive.arcadeDrive(forwardSpeed, rotationSpeed); - } - - //////////////////////////////////////////////////// - // Simulation support - - DrivetrainSim dtSim; - VisionSim visionSim; - - @Override - public void simulationInit() { - dtSim = new DrivetrainSim(leftMotor, rightMotor); - visionSim = new VisionSim("main", camera); - } - - @Override - public void simulationPeriodic() { - dtSim.update(); - visionSim.update(dtSim.getPose()); - } -} diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java deleted file mode 100644 index 2a9c532e8..000000000 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java +++ /dev/null @@ -1,105 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot.sim; - -import static frc.robot.Constants.*; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; -import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; -import edu.wpi.first.wpilibj.simulation.PWMSim; -import frc.robot.Robot; - -/** - * This class handles the simulation of robot physics, sensors, and motor controllers. - * - *

This class and its methods are only relevant during simulation. While on the real robot, the - * real motors/sensors/physics are used instead. - */ -public class DrivetrainSim { - // ----- Simulation specific constants - // Drivetrain plant and simulation. This will calculate how the robot pose changes over time as - // motor voltages are applied. - private static final LinearSystem drivetrainSystem = - LinearSystemId.identifyDrivetrainSystem( - LINEAR_KV, LINEAR_KA, ANGULAR_KV, ANGULAR_KA, TRACKWIDTH_METERS); - private static final DifferentialDrivetrainSim drivetrainSimulator = - new DifferentialDrivetrainSim( - drivetrainSystem, - DRIVE_MOTORS, - GEARING, - TRACKWIDTH_METERS, - WHEEL_DIAMETER_METERS / 2.0, - null); - // ----- - - // PWM handles for getting commanded motor controller speeds - private final PWMSim leftLeader; - private final PWMSim rightLeader; - - public DrivetrainSim(PWMMotorController leftController, PWMMotorController rightController) { - leftLeader = new PWMSim(leftController); - rightLeader = new PWMSim(rightController); - } - - /** - * Perform all periodic drivetrain simulation related tasks to advance our simulation of robot - * physics forward by a single 20ms step. - */ - public void update() { - double leftMotorCmd = 0; - double rightMotorCmd = 0; - - if (DriverStation.isEnabled() && !RobotController.isBrownedOut()) { - leftMotorCmd = leftLeader.getSpeed(); - rightMotorCmd = rightLeader.getSpeed(); - } - - drivetrainSimulator.setInputs( - leftMotorCmd * RobotController.getBatteryVoltage(), - -rightMotorCmd * RobotController.getBatteryVoltage()); - - drivetrainSimulator.update(Robot.kDefaultPeriod); - } - - public Pose2d getPose() { - return drivetrainSimulator.getPose(); - } - - /** - * Resets the simulation back to a pre-defined pose. Useful to simulate the action of placing the - * robot onto a specific spot in the field (e.g. at the start of each match). - * - * @param pose - */ - public void resetPose(Pose2d pose) { - drivetrainSimulator.setPose(pose); - } -} diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java deleted file mode 100644 index 0be5357c6..000000000 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/VisionSim.java +++ /dev/null @@ -1,127 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.robot.sim; - -import static frc.robot.Constants.*; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import java.util.List; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.simulation.VisionTargetSim; - -/** - * This class handles the simulation of the camera and vision target on the field. Updating this - * class will update the camera data, reflecting what the simulated camera sees. - * - *

This class and its methods are only relevant during simulation. While on the real robot, the - * real camera data is used instead. - */ -public class VisionSim { - // ----- Simulation specific constants - // 2020 High goal target shape - // See - // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf - // page 208 - private static final TargetModel kTargetModel = - new TargetModel( - List.of( - new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)), - new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)), - new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)), - new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5)))); - - // Simulated camera properties. These can be set to mimic your actual camera. - private static final int kResolutionWidth = 640; // pixels - private static final int kResolutionHeight = 480; // pixels - private static final Rotation2d kFOVDiag = Rotation2d.fromDegrees(100.0); // degrees - private static final double kAvgErrorPx = 0.2; - private static final double kErrorStdDevPx = 0.05; - private static final double kFPS = 25; - private static final double kAvgLatencyMs = 30; - private static final double kLatencyStdDevMs = 4; - private static final double kMinTargetArea = 0.1; // percentage (0 - 100) - private static final double kMaxLEDRange = 15; // meters - // ----- - - // A simulated vision system which handles simulated cameras and targets. - private VisionSystemSim visionSim; - // The simulation of our PhotonCamera, which will simulate camera frames and target info. - private PhotonCameraSim cameraSim; - - public VisionSim(String name, PhotonCamera camera) { - visionSim = new VisionSystemSim(name); - // Make the vision target visible to this simulated field. - var visionTarget = new VisionTargetSim(TARGET_POSE, kTargetModel); - visionSim.addVisionTargets(visionTarget); - - // Create simulated camera properties from our defined constants. - var cameraProp = new SimCameraProperties(); - cameraProp.setCalibration(kResolutionWidth, kResolutionHeight, kFOVDiag); - cameraProp.setCalibError(kAvgErrorPx, kErrorStdDevPx); - cameraProp.setFPS(kFPS); - cameraProp.setAvgLatencyMs(kAvgLatencyMs); - cameraProp.setLatencyStdDevMs(kLatencyStdDevMs); - // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible - // targets. - cameraSim = new PhotonCameraSim(camera, cameraProp, kMinTargetArea, kMaxLEDRange); - - // Add the simulated camera to view the targets on this simulated field. - visionSim.addCamera( - cameraSim, - new Transform3d( - new Translation3d(0, 0, CAMERA_HEIGHT_METERS), - new Rotation3d(0, -CAMERA_PITCH_RADIANS, 0))); - - cameraSim.enableDrawWireframe(true); - } - - /** - * Update the simulated camera data based on its new field pose. - * - * @param simRobotPose The pose of the simulated robot - */ - public void update(Pose2d simRobotPose) { - visionSim.update(simRobotPose); - } - - /** - * Resets the simulation back to a pre-defined pose. Useful to simulate the action of placing the - * robot onto a specific spot in the field (e.g. at the start of each match). - * - * @param pose - */ - public void resetPose(Pose2d pose) { - visionSim.resetRobotPose(pose); - } -} diff --git a/photonlib-java-examples/swervedriveposeestsim/2023-field.json b/photonlib-java-examples/swervedriveposeestsim/2023-field.json deleted file mode 100644 index ff4c5ca23..000000000 --- a/photonlib-java-examples/swervedriveposeestsim/2023-field.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "game": "Charged Up", - "field-image": "2023-field.png", - "field-corners": { - "top-left": [ - 46, - 36 - ], - "bottom-right": [ - 1088, - 544 - ] - }, - "field-size": [ - 54.27083, - 26.2916 - ], - "field-unit": "foot" - } diff --git a/photonlib-java-examples/swervedriveposeestsim/2023-field.png b/photonlib-java-examples/swervedriveposeestsim/2023-field.png deleted file mode 100644 index ab3f0ff33..000000000 Binary files a/photonlib-java-examples/swervedriveposeestsim/2023-field.png and /dev/null differ diff --git a/photonlib-java-examples/swervedriveposeestsim/README.md b/photonlib-java-examples/swervedriveposeestsim/README.md deleted file mode 100644 index 989e07be2..000000000 --- a/photonlib-java-examples/swervedriveposeestsim/README.md +++ /dev/null @@ -1,3 +0,0 @@ -## **`swervedriveposeestsim`** - -### See [PhotonLib Java Examples](../README.md#swervedriveposeestsim) diff --git a/photonlib-java-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-java-examples/swervedriveposeestsim/WPILib-License.md deleted file mode 100644 index 3d5a824ca..000000000 --- a/photonlib-java-examples/swervedriveposeestsim/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-java-examples/swervedriveposeestsim/src/main/deploy/example.txt deleted file mode 100644 index d6ec5cf83..000000000 --- a/photonlib-java-examples/swervedriveposeestsim/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. diff --git a/photonlib-java-examples/simaimandrange/.gitignore b/photonlib-python-examples/aimandrange/.gitignore similarity index 97% rename from photonlib-java-examples/simaimandrange/.gitignore rename to photonlib-python-examples/aimandrange/.gitignore index 9912213c2..244ad813e 100644 --- a/photonlib-java-examples/simaimandrange/.gitignore +++ b/photonlib-python-examples/aimandrange/.gitignore @@ -158,6 +158,18 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json networktables.json + +__pycache__ 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 000000000..f54f38af6 --- /dev/null +++ b/photonlib-python-examples/aimandrange/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} diff --git a/photonlib-python-examples/aimandrange/drivetrain.py b/photonlib-python-examples/aimandrange/drivetrain.py new file mode 100644 index 000000000..d411b7b8b --- /dev/null +++ b/photonlib-python-examples/aimandrange/drivetrain.py @@ -0,0 +1,202 @@ +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + + +import math +import wpilib +import wpilib.simulation +import wpimath.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) diff --git a/photonlib-python-examples/aimandrange/pyproject.toml b/photonlib-python-examples/aimandrange/pyproject.toml new file mode 100644 index 000000000..07d7ac969 --- /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 000000000..617f0ccba --- /dev/null +++ b/photonlib-python-examples/aimandrange/robot.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + +import 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 +CAM_MOUNT_HEIGHT_m = 0.5 # Measured with a tape measure, or in CAD +CAM_MOUNT_PITCH_deg = -30.0 # Measured with a protractor, or in CAD +TAG_7_MOUNT_HEIGHT_m = 1.435 # From the 2024 game manual + + +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 + # 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 = CAM_MOUNT_HEIGHT_m - TAG_7_MOUNT_HEIGHT_m + angleDelta = math.radians(CAM_MOUNT_PITCH_deg - 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() diff --git a/photonlib-cpp-examples/apriltagExample/simgui-ds.json b/photonlib-python-examples/aimandrange/simgui-ds.json similarity index 94% rename from photonlib-cpp-examples/apriltagExample/simgui-ds.json rename to photonlib-python-examples/aimandrange/simgui-ds.json index 7b5100502..addd5860c 100644 --- a/photonlib-cpp-examples/apriltagExample/simgui-ds.json +++ b/photonlib-python-examples/aimandrange/simgui-ds.json @@ -1,13 +1,11 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ - {}, + { + "decKey": 65, + "incKey": 68 + }, { "decKey": 87, "incKey": 83 @@ -18,8 +16,8 @@ }, {}, { - "decKey": 65, - "incKey": 68 + "decKey": 81, + "incKey": 69 } ], "axisCount": 6, diff --git a/photonlib-python-examples/aimandrange/simgui.json b/photonlib-python-examples/aimandrange/simgui.json new file mode 100644 index 000000000..2a9feb9f7 --- /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 000000000..25990c839 Binary files /dev/null and b/photonlib-python-examples/aimandrange/swerve_module.png differ diff --git a/photonlib-python-examples/aimandrange/swervemodule.py b/photonlib-python-examples/aimandrange/swervemodule.py new file mode 100644 index 000000000..803a1f5f3 --- /dev/null +++ b/photonlib-python-examples/aimandrange/swervemodule.py @@ -0,0 +1,218 @@ +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + +import math +import wpilib +import wpilib.simulation +import wpimath.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 000000000..e19514714 --- /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-java-examples/getinrange/.gitignore b/photonlib-python-examples/aimattarget/.gitignore similarity index 96% rename from photonlib-java-examples/getinrange/.gitignore rename to photonlib-python-examples/aimattarget/.gitignore index 9535c8303..244ad813e 100644 --- a/photonlib-java-examples/getinrange/.gitignore +++ b/photonlib-python-examples/aimattarget/.gitignore @@ -158,5 +158,18 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json +networktables.json + +__pycache__ 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 000000000..f54f38af6 --- /dev/null +++ b/photonlib-python-examples/aimattarget/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} diff --git a/photonlib-python-examples/aimattarget/drivetrain.py b/photonlib-python-examples/aimattarget/drivetrain.py new file mode 100644 index 000000000..d411b7b8b --- /dev/null +++ b/photonlib-python-examples/aimattarget/drivetrain.py @@ -0,0 +1,202 @@ +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + + +import math +import wpilib +import wpilib.simulation +import wpimath.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) diff --git a/photonlib-python-examples/aimattarget/pyproject.toml b/photonlib-python-examples/aimattarget/pyproject.toml new file mode 100644 index 000000000..07d7ac969 --- /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 000000000..3f66961c1 --- /dev/null +++ b/photonlib-python-examples/aimattarget/robot.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + + +import 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 + 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() diff --git a/photonlib-java-examples/getinrange/simgui-ds.json b/photonlib-python-examples/aimattarget/simgui-ds.json similarity index 93% rename from photonlib-java-examples/getinrange/simgui-ds.json rename to photonlib-python-examples/aimattarget/simgui-ds.json index 69b1a3cbc..addd5860c 100644 --- a/photonlib-java-examples/getinrange/simgui-ds.json +++ b/photonlib-python-examples/aimattarget/simgui-ds.json @@ -11,13 +11,16 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/photonlib-python-examples/aimattarget/simgui.json b/photonlib-python-examples/aimattarget/simgui.json new file mode 100644 index 000000000..2a9feb9f7 --- /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 000000000..25990c839 Binary files /dev/null and b/photonlib-python-examples/aimattarget/swerve_module.png differ diff --git a/photonlib-python-examples/aimattarget/swervemodule.py b/photonlib-python-examples/aimattarget/swervemodule.py new file mode 100644 index 000000000..803a1f5f3 --- /dev/null +++ b/photonlib-python-examples/aimattarget/swervemodule.py @@ -0,0 +1,218 @@ +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + +import math +import wpilib +import wpilib.simulation +import wpimath.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 000000000..e19514714 --- /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/.gitignore b/photonlib-python-examples/poseest/.gitignore new file mode 100644 index 000000000..244ad813e --- /dev/null +++ b/photonlib-python-examples/poseest/.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__ diff --git a/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json b/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json new file mode 100644 index 000000000..f54f38af6 --- /dev/null +++ b/photonlib-python-examples/poseest/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1736} diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py new file mode 100644 index 000000000..4269061cc --- /dev/null +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -0,0 +1,226 @@ +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + + +import math +import wpilib +import wpilib.simulation +import wpimath.geometry +import wpimath.kinematics +import wpimath.estimator +import swervemodule + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + +kInitialPose = wpimath.geometry.Pose2d( + wpimath.geometry.Translation2d(1.0, 1.0), + wpimath.geometry.Rotation2d.fromDegrees(0.0), +) + + +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.poseEst = wpimath.estimator.SwerveDrive4PoseEstimator( + self.kinematics, + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + kInitialPose, + ) + + 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.poseEst.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + def addVisionPoseEstimate( + self, pose: wpimath.geometry.Pose3d, timestamp: float + ) -> None: + self.poseEst.addVisionMeasurement(pose, timestamp) + + def resetPose(self) -> None: + self.poseEst.resetPosition( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + kInitialPose, + ) + + 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.poseEst.getEstimatedPosition() + 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.poseEst.getEstimatedPosition() + 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.poseEst.getEstimatedPosition()) + 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) diff --git a/photonlib-python-examples/poseest/pyproject.toml b/photonlib-python-examples/poseest/pyproject.toml new file mode 100644 index 000000000..07d7ac969 --- /dev/null +++ b/photonlib-python-examples/poseest/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/poseest/robot.py b/photonlib-python-examples/poseest/robot.py new file mode 100644 index 000000000..5396c93ab --- /dev/null +++ b/photonlib-python-examples/poseest/robot.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + + +import wpilib +import wpimath.geometry +from robotpy_apriltag import AprilTagField, loadAprilTagLayoutField +import drivetrain + +from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy + +kRobotToCam = 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") + self.camPoseEst = PhotonPoseEstimator( + loadAprilTagLayoutField(AprilTagField.k2024Crescendo), + PoseStrategy.LOWEST_AMBIGUITY, + self.cam, + kRobotToCam, + ) + + def robotPeriodic(self) -> None: + camEstPose = self.camPoseEst.update() + if camEstPose: + self.swerve.addVisionPoseEstimate( + camEstPose.estimatedPose, camEstPose.timestampSeconds + ) + + 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 + + self.swerve.drive(xSpeed, ySpeed, rot, True, self.getPeriod()) + + def _simulationPeriodic(self) -> None: + self.swerve.simulationPeriodic() + return super()._simulationPeriodic() diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-python-examples/poseest/simgui-ds.json similarity index 91% rename from photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json rename to photonlib-python-examples/poseest/simgui-ds.json index c4b7efd3d..addd5860c 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json +++ b/photonlib-python-examples/poseest/simgui-ds.json @@ -11,13 +11,16 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, @@ -91,8 +94,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/photonlib-python-examples/poseest/simgui.json b/photonlib-python-examples/poseest/simgui.json new file mode 100644 index 000000000..2a9feb9f7 --- /dev/null +++ b/photonlib-python-examples/poseest/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/poseest/swerve_module.png b/photonlib-python-examples/poseest/swerve_module.png new file mode 100644 index 000000000..25990c839 Binary files /dev/null and b/photonlib-python-examples/poseest/swerve_module.png differ diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py new file mode 100644 index 000000000..02b9987fa --- /dev/null +++ b/photonlib-python-examples/poseest/swervemodule.py @@ -0,0 +1,211 @@ +################################################################################### +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +################################################################################### + +import math +import wpilib +import wpilib.simulation +import wpimath.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) + + # 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() + ) + + self.driveMotor.setVoltage(driveOutput + driveFeedforward) + self.turningMotor.setVoltage(turnOutput) + + 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 / 0.7 + 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/poseest/tests/pyfrc_test.py b/photonlib-python-examples/poseest/tests/pyfrc_test.py new file mode 100644 index 000000000..e19514714 --- /dev/null +++ b/photonlib-python-examples/poseest/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/run.bat b/photonlib-python-examples/run.bat new file mode 100644 index 000000000..dcb872588 --- /dev/null +++ b/photonlib-python-examples/run.bat @@ -0,0 +1,25 @@ +@echo off +setlocal + +:: Check if the first argument is provided +if "%~1"=="" ( + echo Error: No example-to-run provided. + exit /b 1 +) + +:: To run any example, we want to use photonlib out of the source code in this repo. +:: Build the wheel first +pushd %~dp0..\photon-lib\py +if exist build rmdir /S /Q build +python setup.py bdist_wheel +popd + +:: Add the output directory to PYTHONPATH to make sure it gets picked up ahead of any other installs +set PHOTONLIBPY_ROOT=%~dp0..\photon-lib\py +set PYTHONPATH=%PHOTONLIBPY_ROOT% + +:: Move to to the right example folder +cd %~1 + +:: Run the example +robotpy sim