Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Examples Clean-Up (#1408)

Browse Source
This commit is contained in:
Chris Gerth
2024-09-14 23:10:02 -05:00
committed by GitHub
parent 596c87519c
commit 9e6a066561
269 changed files with 9346 additions and 3734 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
docs/source/docs/additional-resources/best-practices.md
View File

@@ -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.

7
docs/source/docs/apriltag-pipelines/multitag.md
View File

@@ -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}

38
docs/source/docs/contributing/building-photon.md
View File

@@ -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 <example-name>: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 <example name>
```

2
docs/source/docs/description.md
View File

@@ -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

37
docs/source/docs/examples/aimandrange.md
View File

@@ -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 <docs/examples/aimingatatarget:Knowledge and Equipment Needed>` and {ref}`Getting in Range of the Target <docs/examples/gettinginrangeofthetarget:Knowledge and Equipment Needed>`.
- Everything required in {ref}`Aiming at a Target <docs/examples/aimingatatarget:Knowledge and Equipment Needed>`.
## 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
```

42
docs/source/docs/examples/aimingatatarget.md
View File

@@ -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 <docs/additional-resources/nt-api:Getting Target Information>` and the documentation for PhotonLib {ref}`here <docs/programming/photonlib/adding-vendordep:What is PhotonLib?>`.
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 <docs/additional-resources/nt-api:Getting Target Information>` and the documentation for PhotonLib {ref}`here <docs/programming/photonlib/adding-vendordep:What is PhotonLib?>`.
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
```

58
docs/source/docs/examples/gettinginrangeofthetarget.md
View File

@@ -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 <docs/examples/aimingatatarget:Knowledge and Equipment Needed>`.
- 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.
:::

BIN
docs/source/docs/examples/images/poseest_demo.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 24 MiB

4
docs/source/docs/examples/index.md
View File

@@ -4,8 +4,6 @@
:maxdepth: 1
aimingatatarget
gettinginrangeofthetarget
aimandrange
simaimandrange
simposeest
poseest
```

211
docs/source/docs/examples/poseest.md Normal file
View File

@@ -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 <docs/examples/aimandrange:Knowledge and Equipment Needed>`, 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
```

96
docs/source/docs/examples/simaimandrange.md
View File

@@ -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 <docs/examples/aimandrange:Knowledge and Equipment Needed>`.
## 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
<video width="85%" controls>
<source src="../../_static/assets/simaimandrange.mp4" type="video/mp4">
Your browser does not support the video tag.
</video>
```
## 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.

133
docs/source/docs/examples/simposeest.md
View File

@@ -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 <docs/examples/aimandrange:Knowledge and Equipment Needed>`, 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.

6
docs/source/docs/installation/networking.md
View File

@@ -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}

2
docs/source/docs/installation/sw_install/other-coprocessors.md
View File

@@ -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}

65
docs/source/docs/integration/advancedStrategies.md
View File

@@ -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 <docs/programming/photonlib/robot-pose-estimator:AprilTags and PhotonPoseEstimator>`. Alternatively, {ref}`a "multi-tag" strategy can do this process on the coprocessor. <docs/apriltag-pipelines/multitag:Enabling MultiTag>`.
## 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 <docs/examples/simposeest:Knowledge and Equipment Needed>` 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 <docs/examples/poseest:Knowledge and Equipment Needed>` example for details on integrating this.

46
docs/source/docs/integration/aprilTagStrategies.md
View File

@@ -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.

1
docs/source/docs/integration/index.md
View File

@@ -6,5 +6,4 @@
background
simpleStrategies
advancedStrategies
aprilTagStrategies
```

26
docs/source/docs/integration/simpleStrategies.md
View File

@@ -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 <docs/examples/aimingatatarget:Knowledge and Equipment Needed>` 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 <docs/examples/gettinginrangeofthetarget:Knowledge and Equipment Needed>` 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 <docs/examples/aimandrange:Knowledge and Equipment Needed>` example for more information.

8
docs/source/docs/programming/photonlib/controlling-led.md
View File

@@ -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!
```

20
docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md
View File

@@ -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}

40
docs/source/docs/programming/photonlib/getting-target-data.md
View File

@@ -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<PhotonTrackedTarget> targets = result.getTargets();
.. code-block:: c++
.. code-block:: C++
// Get a list of currently tracked targets.
wpi::ArrayRef<photonlib::PhotonTrackedTarget> 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 <docs/reflectiveAndShape/contour-filtering:Con
```{eval-rst}
.. tab-set-code::
.. code-block:: java
.. code-block:: Java
// Get the current best target.
PhotonTrackedTarget target = result.getBestTarget();
.. code-block:: c++
.. code-block:: C++
// Get the current best target.
photonlib::PhotonTrackedTarget target = result.GetBestTarget();
.. code-block:: python
.. code-block:: Python
# TODO - Not currently supported
# Coming Soon!
```
@@ -149,7 +149,7 @@ You can get the {ref}`best target <docs/reflectiveAndShape/contour-filtering:Con
```{eval-rst}
.. tab-set-code::
.. code-block:: java
.. code-block:: Java
// Get information from target.
double yaw = target.getYaw();
@@ -159,7 +159,7 @@ You can get the {ref}`best target <docs/reflectiveAndShape/contour-filtering:Con
Transform2d pose = target.getCameraToTarget();
List<TargetCorner> 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 <docs/reflectiveAndShape/contour-filtering:Con
frc::Transform2d pose = target.GetCameraToTarget();
wpi::SmallVector<std::pair<double, double>, 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()

35
docs/source/docs/programming/photonlib/robot-pose-estimator.md
View File

@@ -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<photonlib::PhotonCamera> 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<frc::Pose2d, units::millisecond_t> 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

59
docs/source/docs/programming/photonlib/using-target-data.md
View File

@@ -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!
```

4
docs/source/docs/reflectiveAndShape/3D.md
View File

@@ -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 <docs/calibration/calibration:Calibrating Your Camera>` 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 <docs/calibration/calibration:Calibrating Your Camera>` 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
</video>
```
## 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.

2
docs/source/docs/simulation/hardware-in-the-loop-sim.md
View File

@@ -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 <docs/installation/index:Installation & Setup>` for all devices.
Before continuing, ensure PhotonVision is installed on your device. Instructions can be found {ref}`here <docs/installation/index:Installation & Setup>` 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.

5
docs/source/docs/simulation/index.md
View File

@@ -4,7 +4,8 @@
:maxdepth: 0
:titlesonly: true
simulation
simulation-deprecated
simulation-java
simulation-cpp
simulation-python
hardware-in-the-loop-sim
```

5
docs/source/docs/simulation/simulation-cpp.md Normal file
View File

@@ -0,0 +1,5 @@
# Simulation Support in PhotonLib in C++
## What Is Supported?
Nothing yet.

103
docs/source/docs/simulation/simulation-deprecated.md
View File

@@ -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<PhotonTrackedTarget> visibleTgtList = new ArrayList<PhotonTrackedTarget>();
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.

35
docs/source/docs/simulation/simulation.md → docs/source/docs/simulation/simulation-java.md
View File

@@ -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*
:::

5
docs/source/docs/simulation/simulation-python.md Normal file
View File

@@ -0,0 +1,5 @@
# Simulation Support in PhotonLib in Python
## What Is Supported?
Nothing Yet

2
docs/source/index.md
View File

@@ -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

4
photon-lib/py/disableUsingDevBuilds.bat Normal file
View File

@@ -0,0 +1,4 @@
@echo off
setlocal
python -m pip config unset global.find-links

4
photon-lib/py/enableUsingDevBuilds.bat Normal file
View File

@@ -0,0 +1,4 @@
@echo off
setlocal
python -m pip config set global.find-links %~dp0\dist

17
photon-lib/py/photonlibpy/__init__.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
from .packet import Packet # noqa
from .estimatedRobotPose import EstimatedRobotPose # noqa

17
photon-lib/py/photonlibpy/estimatedRobotPose.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
from dataclasses import dataclass
from typing import TYPE_CHECKING

1
photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
View File

@@ -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;"

1
photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py
View File

@@ -24,7 +24,6 @@ from ..targeting import *
class PhotonPipelineMetadataSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2"
MESSAGE_FORMAT = (

1
photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
View File

@@ -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;"

1
photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py
View File

@@ -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;"

1
photon-lib/py/photonlibpy/generated/PnpResultSerde.py
View File

@@ -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;"

1
photon-lib/py/photonlibpy/generated/TargetCornerSerde.py
View File

@@ -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;"

17
photon-lib/py/photonlibpy/packet.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
import struct
from typing import Any, Optional, Type
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion

17
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
from enum import Enum
from typing import List
import ntcore

17
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
import enum
from typing import Optional

17
photon-lib/py/test/photonPoseEstimator_test.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
# from photonlibpy import MultiTargetPNPResult, PnpResult
# from photonlibpy import PhotonPipelineResult
# from photonlibpy import PhotonPoseEstimator, PoseStrategy

18
photon-lib/py/test/photonlibpy_test.py
View File

@@ -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 <https://www.gnu.org/licenses/>.
###############################################################################
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")

4
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -87,8 +87,8 @@ class PhotonCamera {
*/
std::vector<PhotonPipelineResult> GetAllUnreadResults();
[[deprecated("Replace with GetAllUnreadResults")]]
PhotonPipelineResult GetLatestResult();
[[deprecated("Replace with GetAllUnreadResults")]] PhotonPipelineResult
GetLatestResult();
/**
* Toggles driver mode.

7
photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
View File

@@ -53,8 +53,8 @@ static std::vector<cv::Point2f> GetConvexHull(
return convexPoints;
}
[[maybe_unused]]
static cv::RotatedRect GetMinAreaRect(const std::vector<cv::Point2f>& points) {
[[maybe_unused]] static cv::RotatedRect GetMinAreaRect(
const std::vector<cv::Point2f>& points) {
return cv::minAreaRect(points);
}
@@ -144,8 +144,7 @@ static std::vector<cv::Point3f> RotationToRVec(
return cv::boundingRect(points);
}
[[maybe_unused]]
static std::vector<cv::Point2f> ProjectPoints(
[[maybe_unused]] static std::vector<cv::Point2f> ProjectPoints(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,
const RotTrlTransform3d& camRt,

3
photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
View File

@@ -32,8 +32,7 @@
namespace photon {
namespace VisionEstimation {
[[maybe_unused]]
static std::vector<frc::AprilTag> GetVisibleLayoutTags(
[[maybe_unused]] static std::vector<frc::AprilTag> GetVisibleLayoutTags(
const std::vector<PhotonTrackedTarget>& visTags,
const frc::AprilTagFieldLayout& layout) {
std::vector<frc::AprilTag> retVal{};

4
photonlib-cpp-examples/aimandrange/build.gradle
View File

@@ -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)

0
photonlib-cpp-examples/apriltagExample/networktables.json → photonlib-cpp-examples/aimandrange/networktables.json
View File

2
photonlib-cpp-examples/aimandrange/settings.gradle
View File

@@ -1,6 +1,6 @@
import org.gradle.internal.os.OperatingSystem
rootProject.name = 'aimandrange'
rootProject.name = 'aimattarget'
pluginManagement {
repositories {

7
photonlib-cpp-examples/aimandrange/simgui-ds.json
View File

@@ -11,10 +11,13 @@
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
},
{},
{
"decKey": 81,
"incKey": 69
}
],
"axisCount": 6,

52
photonlib-cpp-examples/aimandrange/simgui-window.json
View File

@@ -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"
}
}
}

155
photonlib-cpp-examples/aimandrange/simgui.json
View File

@@ -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
}
}
}

126
photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp
View File

@@ -26,44 +26,106 @@
#include <photon/PhotonUtils.h>
#include <iostream>
#include <frc/simulation/BatterySim.h>
#include <frc/simulation/RoboRioSim.h>
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

200
photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDrive.cpp Normal file
View File

@@ -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 <iostream>
#include <string>
#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SmartDashboard.h>
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<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
bool steerInPlace) {
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&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<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
std::array<frc::SwerveModuleState, 4> 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<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
const {
std::array<frc::SwerveModulePosition, 4> 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<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
std::array<frc::Pose2d, 4> 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<double>());
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
frc::SmartDashboard::PutNumber(table + "Heading",
pose.Rotation().Degrees().to<double>());
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
frc::SmartDashboard::PutNumber(
table + "Omega Degrees",
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
frc::SmartDashboard::PutNumber(table + "Target VX",
targetChassisSpeeds.vx.to<double>());
frc::SmartDashboard::PutNumber(table + "Target VY",
targetChassisSpeeds.vy.to<double>());
frc::SmartDashboard::PutNumber(
table + "Target Omega Degrees",
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
.to<double>());
for (auto& module : swerveMods) {
module.Log();
}
}
void SwerveDrive::SimulationPeriodic() {
std::array<units::volt_t, 4> driveInputs;
std::array<units::volt_t, 4> 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<units::ampere_t, 4> driveCurrents =
swerveDriveSim.GetDriveCurrentDraw();
for (const auto& current : driveCurrents) {
totalCurrentDraw += current;
}
std::array<units::ampere_t, 4> 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; }

0
photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp → photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveDriveSim.cpp
View File

0
photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp → photonlib-cpp-examples/aimandrange/src/main/cpp/subsystems/SwerveModule.cpp
View File

0
photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt → photonlib-cpp-examples/aimandrange/src/main/deploy/example.txt
View File

118
photonlib-cpp-examples/aimandrange/src/main/include/Constants.h Normal file
View File

@@ -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 <numbers>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Transform3d.h>
#include <frc/geometry/Translation2d.h>
#include <units/length.h>
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<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
inline const Eigen::Matrix<double, 3, 1> 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<units::meters> kDriveFF{
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
inline const frc::SimpleMotorFeedforward<units::radians> 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

60
photonlib-cpp-examples/aimandrange/src/main/include/Robot.h
View File

@@ -28,44 +28,36 @@
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
#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;
};

87
photonlib-cpp-examples/aimandrange/src/main/include/VisionSim.h Normal file
View File

@@ -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 <photon/PhotonCamera.h>
#include <photon/PhotonPoseEstimator.h>
#include <photon/estimation/VisionEstimation.h>
#include <photon/simulation/VisionSystemSim.h>
#include <photon/simulation/VisionTargetSim.h>
#include <photon/targeting/PhotonPipelineResult.h>
#include <limits>
#include <memory>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include "Constants.h"
class VisionSim {
public:
explicit VisionSim(photon::PhotonCamera* camera) {
if (frc::RobotBase::IsSimulation()) {
visionSim = std::make_unique<photon::VisionSystemSim>("main");
visionSim->AddAprilTags(constants::Vision::kTagLayout);
cameraProp = std::make_unique<photon::SimCameraProperties>();
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<photon::PhotonCameraSim>(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<photon::VisionSystemSim> visionSim;
std::unique_ptr<photon::SimCameraProperties> cameraProp;
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
// The most recent result, cached for calculating std devs
photon::PhotonPipelineResult m_latestResult;
};

80
photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDrive.h Normal file
View File

@@ -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 <frc/ADXRS450_Gyro.h>
#include <frc/SPI.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/simulation/ADXRS450_GyroSim.h>
#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<frc::SwerveModuleState, 4>& 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<frc::SwerveModuleState, 4> GetModuleStates() const;
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
std::array<frc::Pose2d, 4> GetModulePoses() const;
void Log();
void SimulationPeriodic();
frc::Pose2d GetSimPose() const;
units::ampere_t GetCurrentDraw() const;
private:
std::array<SwerveModule, 4> 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};
};

0
photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h → photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveDriveSim.h
View File

0
photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h → photonlib-cpp-examples/aimandrange/src/main/include/subsystems/SwerveModule.h
View File

0
photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png → photonlib-cpp-examples/aimandrange/swerve_module.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png → photonlib-cpp-examples/aimandrange/tag-blue.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 4.6 KiB

After

Width:  |  Height:  |  Size: 4.6 KiB

0
photonlib-cpp-examples/swervedriveposeestsim/tag-green.png → photonlib-cpp-examples/aimandrange/tag-green.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 4.6 KiB

After

Width:  |  Height:  |  Size: 4.6 KiB

34
photonlib-cpp-examples/aimattarget/build.gradle
View File

@@ -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)

0
photonlib-cpp-examples/swervedriveposeestsim/networktables.json → photonlib-cpp-examples/aimattarget/networktables.json
View File

7
photonlib-cpp-examples/aimattarget/simgui-ds.json
View File

@@ -11,10 +11,13 @@
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
},
{},
{
"decKey": 81,
"incKey": 69
}
],
"axisCount": 6,

50
photonlib-cpp-examples/aimattarget/simgui-window.json
View File

@@ -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"
}
}
}

159
photonlib-cpp-examples/aimattarget/simgui.json
View File

@@ -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
}
}
}
}

110
photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp
View File

@@ -24,50 +24,98 @@
#include "Robot.h"
#include <photon/PhotonUtils.h>
#include <iostream>
#include <frc/Timer.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/time.h>
#include <frc/simulation/BatterySim.h>
#include <frc/simulation/RoboRioSim.h>
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<double>(),
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<double>());
// 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

200
photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDrive.cpp Normal file
View File

@@ -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 <iostream>
#include <string>
#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SmartDashboard.h>
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<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
bool steerInPlace) {
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&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<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
std::array<frc::SwerveModuleState, 4> 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<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
const {
std::array<frc::SwerveModulePosition, 4> 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<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
std::array<frc::Pose2d, 4> 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<double>());
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
frc::SmartDashboard::PutNumber(table + "Heading",
pose.Rotation().Degrees().to<double>());
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
frc::SmartDashboard::PutNumber(
table + "Omega Degrees",
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
frc::SmartDashboard::PutNumber(table + "Target VX",
targetChassisSpeeds.vx.to<double>());
frc::SmartDashboard::PutNumber(table + "Target VY",
targetChassisSpeeds.vy.to<double>());
frc::SmartDashboard::PutNumber(
table + "Target Omega Degrees",
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
.to<double>());
for (auto& module : swerveMods) {
module.Log();
}
}
void SwerveDrive::SimulationPeriodic() {
std::array<units::volt_t, 4> driveInputs;
std::array<units::volt_t, 4> 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<units::ampere_t, 4> driveCurrents =
swerveDriveSim.GetDriveCurrentDraw();
for (const auto& current : driveCurrents) {
totalCurrentDraw += current;
}
std::array<units::ampere_t, 4> 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; }

285
photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveDriveSim.cpp Normal file
View File

@@ -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 <iostream>
#include <frc/RobotController.h>
#include <frc/system/Discretization.h>
template <typename T>
int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
SwerveDriveSim::SwerveDriveSim(
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
const frc::DCMotor& driveMotor, double driveGearing,
units::meter_t driveWheelRadius,
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
const frc::DCMotor& steerMotor, double steerGearing,
const frc::SwerveDriveKinematics<numModules>& kinematics)
: SwerveDriveSim(
frc::LinearSystem<2, 1, 2>{
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
-driveFF.kV.to<double>() / driveFF.kA.to<double>())
.finished(),
Eigen::Matrix<double, 2, 1>{0.0, 1.0 / driveFF.kA.to<double>()},
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
Eigen::Matrix<double, 2, 1>{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<double>() / steerFF.kA.to<double>())
.finished(),
Eigen::Matrix<double, 2, 1>{0.0, 1.0 / steerFF.kA.to<double>()},
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
Eigen::Matrix<double, 2, 1>{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<numModules>& 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<units::volt_t, numModules>& 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<units::volt_t, numModules>& 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<double, 2, 1> SwerveDriveSim::CalculateX(
const Eigen::Matrix<double, 2, 2>& discA,
const Eigen::Matrix<double, 2, 1>& discB,
const Eigen::Matrix<double, 2, 1>& 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<double>(), kS.to<double>());
nextStateVel += discB(1, 0) * ksSystemEffect;
inputToStop = nextStateVel / -discB(1, 0);
double signToStop = sgn(inputToStop);
double inputSign = sgn(input.to<double>());
double ksInputEffect = 0;
if (std::abs(ksSystemEffect) < kS.to<double>()) {
double absInput = std::abs(input.to<double>());
ksInputEffect =
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
} else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
double absInput = std::abs(input.to<double>() - inputToStop);
ksInputEffect =
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
}
auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
ksInputEffect};
auto Bu = discB * sF;
auto retVal = Ax + Bu;
return retVal;
}
void SwerveDriveSim::Update(units::second_t dt) {
Eigen::Matrix<double, 2, 2> driveDiscA;
Eigen::Matrix<double, 2, 1> driveDiscB;
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
&driveDiscB);
Eigen::Matrix<double, 2, 2> steerDiscA;
Eigen::Matrix<double, 2, 1> steerDiscB;
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
&steerDiscB);
std::array<frc::SwerveModulePosition, 4> 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<double, 2, 1>{0, 0};
steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
}
omega = 0_rad_per_s;
}
}
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
const std::array<Eigen::Matrix<double, 2, 1>,
numModules>& moduleDriveStates,
const std::array<Eigen::Matrix<double, 2, 1>,
numModules>& moduleSteerStates) {
this->pose = pose;
driveStates = moduleDriveStates;
steerStates = moduleSteerStates;
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
}
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
std::array<frc::SwerveModulePosition, numModules>
SwerveDriveSim::GetModulePositions() const {
std::array<frc::SwerveModulePosition, numModules> 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<frc::SwerveModulePosition, numModules>
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
units::radian_t steerStdDev) {
std::array<frc::SwerveModulePosition, numModules> 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<frc::SwerveModuleState, numModules>
SwerveDriveSim::GetModuleStates() {
std::array<frc::SwerveModuleState, numModules> 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<Eigen::Matrix<double, 2, 1>, numModules>
SwerveDriveSim::GetDriveStates() const {
return driveStates;
}
std::array<Eigen::Matrix<double, 2, 1>, 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<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
const {
std::array<units::ampere_t, numModules> 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<double>();
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
frc::RobotController::GetBatteryVoltage());
}
return currents;
}
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
const {
std::array<units::ampere_t, numModules> 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;
}

147
photonlib-cpp-examples/aimattarget/src/main/cpp/subsystems/SwerveModule.cpp Normal file
View File

@@ -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 <iostream>
#include <string>
#include <frc/MathUtil.h>
#include <frc/RobotController.h>
#include <frc/smartdashboard/SmartDashboard.h>
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<double>());
steerEncoder.SetDistancePerPulse(
constants::Swerve::kSteerRadPerPulse.to<double>());
steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
}
void SwerveModule::Periodic() {
units::volt_t steerPID = units::volt_t{
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
desiredState.angle.Radians().to<double>())};
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<double>())};
}
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<units::degrees>()
.to<double>());
frc::SmartDashboard::PutNumber(
table + "Steer Target Degrees",
units::radian_t{steerPIDController.GetSetpoint()}
.convert<units::degrees>()
.to<double>());
frc::SmartDashboard::PutNumber(
table + "Drive Velocity Feet",
state.speed.convert<units::feet_per_second>().to<double>());
frc::SmartDashboard::PutNumber(
table + "Drive Velocity Target Feet",
desiredState.speed.convert<units::feet_per_second>().to<double>());
frc::SmartDashboard::PutNumber(table + "Drive Current",
driveCurrentSim.to<double>());
frc::SmartDashboard::PutNumber(table + "Steer Current",
steerCurrentSim.to<double>());
}
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<double>());
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
driveCurrentSim = driveCurrent;
steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
steerEncoderSim.SetRate(steerEncoderRate.to<double>());
steerCurrentSim = steerCurrent;
}

4
photonlib-cpp-examples/aimattarget/src/main/deploy/example.txt Normal file
View File

@@ -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.

118
photonlib-cpp-examples/aimattarget/src/main/include/Constants.h Normal file
View File

@@ -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 <numbers>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/geometry/Transform3d.h>
#include <frc/geometry/Translation2d.h>
#include <units/length.h>
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<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
inline const Eigen::Matrix<double, 3, 1> 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<units::meters> kDriveFF{
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
inline const frc::SimpleMotorFeedforward<units::radians> 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

40
photonlib-cpp-examples/aimattarget/src/main/include/Robot.h
View File

@@ -28,27 +28,33 @@
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
#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;
};

87
photonlib-cpp-examples/aimattarget/src/main/include/VisionSim.h Normal file
View File

@@ -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 <photon/PhotonCamera.h>
#include <photon/PhotonPoseEstimator.h>
#include <photon/estimation/VisionEstimation.h>
#include <photon/simulation/VisionSystemSim.h>
#include <photon/simulation/VisionTargetSim.h>
#include <photon/targeting/PhotonPipelineResult.h>
#include <limits>
#include <memory>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include "Constants.h"
class VisionSim {
public:
explicit VisionSim(photon::PhotonCamera* camera) {
if (frc::RobotBase::IsSimulation()) {
visionSim = std::make_unique<photon::VisionSystemSim>("main");
visionSim->AddAprilTags(constants::Vision::kTagLayout);
cameraProp = std::make_unique<photon::SimCameraProperties>();
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<photon::PhotonCameraSim>(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<photon::VisionSystemSim> visionSim;
std::unique_ptr<photon::SimCameraProperties> cameraProp;
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
// The most recent result, cached for calculating std devs
photon::PhotonPipelineResult m_latestResult;
};

80
photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDrive.h Normal file
View File

@@ -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 <frc/ADXRS450_Gyro.h>
#include <frc/SPI.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/simulation/ADXRS450_GyroSim.h>
#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<frc::SwerveModuleState, 4>& 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<frc::SwerveModuleState, 4> GetModuleStates() const;
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
std::array<frc::Pose2d, 4> GetModulePoses() const;
void Log();
void SimulationPeriodic();
frc::Pose2d GetSimPose() const;
units::ampere_t GetCurrentDraw() const;
private:
std::array<SwerveModule, 4> 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};
};

102
photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveDriveSim.h Normal file
View File

@@ -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 <random>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <units/voltage.h>
static constexpr int numModules{4};
class SwerveDriveSim {
public:
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
const frc::DCMotor& driveMotor, double driveGearing,
units::meter_t driveWheelRadius,
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
const frc::DCMotor& steerMotor, double steerGearing,
const frc::SwerveDriveKinematics<numModules>& 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<numModules>& kinematics);
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
static Eigen::Matrix<double, 2, 1> CalculateX(
const Eigen::Matrix<double, 2, 2>& discA,
const Eigen::Matrix<double, 2, 1>& discB,
const Eigen::Matrix<double, 2, 1>& 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<Eigen::Matrix<double, 2, 1>, numModules>&
moduleDriveStates,
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
moduleSteerStates);
frc::Pose2d GetPose() const;
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
units::meter_t driveStdDev, units::radian_t steerStdDev);
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
std::array<Eigen::Matrix<double, 2, 1>, 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<units::ampere_t, numModules> GetDriveCurrentDraw() const;
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
units::ampere_t GetTotalCurrentDraw() const;
private:
std::random_device rd{};
std::mt19937 generator{rd()};
std::normal_distribution<double> 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<numModules> kinematics;
std::array<units::volt_t, numModules> driveInputs{};
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
std::array<units::volt_t, numModules> steerInputs{};
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
frc::Pose2d pose{frc::Pose2d{}};
units::radians_per_second_t omega{0};
};

81
photonlib-cpp-examples/aimattarget/src/main/include/subsystems/SwerveModule.h Normal file
View File

@@ -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 <frc/Encoder.h>
#include <frc/controller/PIDController.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc/simulation/EncoderSim.h>
#include <units/current.h>
#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};
};

0
photonlib-java-examples/swervedriveposeestsim/swerve_module.png → photonlib-cpp-examples/aimattarget/swerve_module.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.9 KiB

After

Width:  |  Height:  |  Size: 2.9 KiB

0
photonlib-java-examples/swervedriveposeestsim/tag-blue.png → photonlib-cpp-examples/aimattarget/tag-blue.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 4.6 KiB

After

Width:  |  Height:  |  Size: 4.6 KiB

0
photonlib-java-examples/swervedriveposeestsim/tag-green.png → photonlib-cpp-examples/aimattarget/tag-green.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 4.6 KiB

After

Width:  |  Height:  |  Size: 4.6 KiB

116
photonlib-cpp-examples/apriltagExample/build.gradle
View File

@@ -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)
}
}
}

81
photonlib-cpp-examples/apriltagExample/simgui-window.json
View File

@@ -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"
}
}
}

40
photonlib-cpp-examples/apriltagExample/simgui.json
View File

@@ -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
}
}

92
photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp
View File

@@ -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 <frc/RobotController.h>
#include <frc/smartdashboard/Field2d.h>
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());
}

61
photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp
View File

@@ -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 <frc/RobotBase.h>
#include <networktables/NetworkTable.h>
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<Robot>(); }
#endif

140
photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h
View File

@@ -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 <numbers>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/estimator/DifferentialDrivePoseEstimator.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/motorcontrol/MotorControllerGroup.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc/simulation/AnalogGyroSim.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/velocity.h>
#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<units::meters> 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};
};

53
photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
View File

@@ -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 <photon/PhotonCamera.h>
#include <photon/PhotonPoseEstimator.h>
#include <utility>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
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<photon::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {
m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
std::optional<photon::EstimatedRobotPose> ret = std::nullopt;
for (const auto& change : camera.GetAllUnreadResults())
ret = m_poseEstimator.Update(change);
return ret;
}
};

50
photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h
View File

@@ -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 <photon/PhotonCamera.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
#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;
};

1
photonlib-cpp-examples/getinrange/.gitignore vendored
View File

@@ -1 +0,0 @@
vendordeps

6
photonlib-cpp-examples/getinrange/.wpilib/wpilib_preferences.json
View File

@@ -1,6 +0,0 @@
{
"enableCppIntellisense": true,
"currentLanguage": "cpp",
"projectYear": "2024",
"teamNumber": 5
}

116
photonlib-cpp-examples/getinrange/build.gradle
View File

@@ -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)
}
}
}

30
photonlib-cpp-examples/getinrange/settings.gradle
View File

@@ -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
}
}
}

Some files were not shown because too many files have changed in this diff Show More