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

Update Java Simulation Examples (#913)

Browse Source 
Removes apriltagExample and simposeest, replacing them with swervedriveposeestsim

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
amquake
2023-10-05 05:57:38 -07:00
committed by GitHub
parent 65d5494ab3
commit ce0d28da93
65 changed files with 1859 additions and 2011 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -66,6 +66,8 @@ public class VisionSystemSim {
private final Field2d dbgField; private final Field2d dbgField;
private final Transform3d kEmptyTrf = new Transform3d();
/** /**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
* running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to
@@ -337,7 +339,7 @@ public class VisionSystemSim {
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted. * determine if a new frame should be submitted.
* *
* @param robotPoseMeters The current robot pose in meters * @param robotPoseMeters The simulated robot pose in meters
*/ */
public void update(Pose2d robotPoseMeters) { public void update(Pose2d robotPoseMeters) {
update(new Pose3d(robotPoseMeters)); update(new Pose3d(robotPoseMeters));
@@ -347,7 +349,7 @@ public class VisionSystemSim {
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted. * determine if a new frame should be submitted.
* *
* @param robotPoseMeters The current robot pose in meters * @param robotPoseMeters The simulated robot pose in meters
*/ */
public void update(Pose3d robotPoseMeters) { public void update(Pose3d robotPoseMeters) {
var targetTypes = targetSets.entrySet(); var targetTypes = targetSets.entrySet();
@@ -370,13 +372,15 @@ public class VisionSystemSim {
var allTargets = new ArrayList<VisionTargetSim>(); var allTargets = new ArrayList<VisionTargetSim>();
targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue())); targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue()));
var visibleTargets = new ArrayList<Pose3d>(); var visTgtPoses2d = new ArrayList<Pose2d>();
var cameraPose2ds = new ArrayList<Pose2d>(); var cameraPoses2d = new ArrayList<Pose2d>();
boolean processed = false;
// process each camera // process each camera
for (var camSim : camSimMap.values()) { for (var camSim : camSimMap.values()) {
// check if this camera is ready to process and get latency // check if this camera is ready to process and get latency
var optTimestamp = camSim.consumeNextEntryTime(); var optTimestamp = camSim.consumeNextEntryTime();
if (optTimestamp.isEmpty()) continue; if (optTimestamp.isEmpty()) continue;
else processed = true;
// when this result "was" read by NT // when this result "was" read by NT
long timestampNT = optTimestamp.get(); long timestampNT = optTimestamp.get();
// this result's processing latency in milliseconds // this result's processing latency in milliseconds
@@ -387,7 +391,7 @@ public class VisionSystemSim {
// use camera pose from the image capture timestamp // use camera pose from the image capture timestamp
Pose3d lateRobotPose = getRobotPose(timestampCapture); Pose3d lateRobotPose = getRobotPose(timestampCapture);
Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get()); Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get());
cameraPose2ds.add(lateCameraPose.toPose2d()); cameraPoses2d.add(lateCameraPose.toPose2d());
// process a PhotonPipelineResult with visible targets // process a PhotonPipelineResult with visible targets
var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets); var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets);
@@ -395,14 +399,12 @@ public class VisionSystemSim {
camSim.submitProcessedFrame(camResult, timestampNT); camSim.submitProcessedFrame(camResult, timestampNT);
// display debug results // display debug results
for (var target : camResult.getTargets()) { for (var target : camResult.getTargets()) {
visibleTargets.add(lateCameraPose.transformBy(target.getBestCameraToTarget())); var trf = target.getBestCameraToTarget();
if (trf.equals(kEmptyTrf)) continue;
visTgtPoses2d.add(lateCameraPose.transformBy(trf).toPose2d());
} }
} }
if (visibleTargets.size() != 0) { if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d);
dbgField if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d);
.getObject("visibleTargetPoses")
.setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList()));
}
if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds);
} }
} }

7
photonlib-cpp-examples/README.md Normal file
View File

@@ -0,0 +1,7 @@
## PhotonLib C++ Examples
### Running examples
For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
---

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

@@ -27,9 +27,11 @@ spotless {
} }
} }
apply from: "examples.gradle"
// Task that depends on the build task for every example // Task that depends on the build task for every example
task buildAllExamples { task -> task buildAllExamples { task ->
new File('examples.txt').eachLine { line -> exampleFolderNames.each { line ->
task.dependsOn(line + ":build") task.dependsOn(line + ":build")
} }
} }

14
photonlib-cpp-examples/examples.gradle Normal file
View File

@@ -0,0 +1,14 @@
// These should be the only 2 non-project subdirectories in the examples folder
// I could check for (it)/build.gradle to exist, but w/e
def EXCLUDED_DIRS = ["bin", "build"]
// List all non-hidden directories not in EXCUDED_DIRS
ext.exampleFolderNames = file("${rootDir}")
.listFiles()
.findAll {
return (it.isDirectory()
&& !it.isHidden()
&& !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
&& it.toPath().resolve("build.gradle").toFile().exists())
}
.collect { it.name }

4
photonlib-cpp-examples/examples.txt
View File

@@ -1,4 +0,0 @@
aimandrange
getinrange
aimattarget
apriltagExample

4
photonlib-cpp-examples/settings.gradle
View File

@@ -1 +1,3 @@
new File('examples.txt').eachLine { line -> include line } apply from: "examples.gradle"
exampleFolderNames.each { line -> include line }

53
photonlib-java-examples/README.md Normal file
View File

@@ -0,0 +1,53 @@
## PhotonLib Java Examples
### Running examples
For instructions on how to run these examples locally, see [Running Examples](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
---
### [**`aimattarget`**](aimattarget)
A simple demonstration of using PhotonVision's 2d target yaw to align a differential drivetrain with a target.
---
### [**`getinrange`**](getinrange)
A simple demonstration of using PhotonVision's 2d target pitch to bring a differential drivetrain to a specific distance from a target.
---
### [**`aimandrange`**](aimandrange)
A combination of the previous `aimattarget` and `getinrange` examples to simultaneously aim and get in range of a target.
---
### [**`simaimandrange`**](simaimandrange)
The above `aimandrange` example with simulation support.
<img src="https://github-production-user-asset-6210df.s3.amazonaws.com/7953350/268856085-432a54b9-f596-4e30-8b57-a8f38f88f985.png" width=60% height=60%>
**Keyboard controls:**
- Drive forward/backward: W/S
- Turn left/right: A/D
- Perform vision alignment: Z
---
### [**`swervedriveposeestsim`**](swervedriveposeestsim)
A minimal swerve drive example demonstrating the usage of PhotonVision for AprilTag vision estimation with a swerve drive pose estimator.
The example also has simulation support with an approximation of swerve drive dynamics.
<img src="https://github-production-user-asset-6210df.s3.amazonaws.com/7953350/268862944-3392e69a-7705-4dbc-9eb8-0d03a6e27b9e.png" width=60% height=60%>
<img src="https://github-production-user-asset-6210df.s3.amazonaws.com/7953350/268857280-bae145b8-356e-4afb-b842-597dbea60df6.png" width=60% height=60%>
**Keyboard controls:**
- Translate field-relative: WASD
- Rotate counter/clockwise: Q/E
- Offset pose estimate: X

3
photonlib-java-examples/aimandrange/README.md Normal file
View File

@@ -0,0 +1,3 @@
## **`aimandrange`**
### See [PhotonLib Java Examples](./README.md#aimandrange)

3
photonlib-java-examples/aimattarget/README.md Normal file
View File

@@ -0,0 +1,3 @@
## **`aimattarget`**
### See [PhotonLib Java Examples](./README.md#aimattarget)

6
photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json
View File

@@ -1,6 +0,0 @@
{
"enableCppIntellisense": false,
"currentLanguage": "java",
"projectYear": "2023Beta",
"teamNumber": 1736
}

94
photonlib-java-examples/apriltagExample/build.gradle
View File

@@ -1,94 +0,0 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2023.4.2"
}
sourceCompatibility = JavaVersion.VERSION_11
targetCompatibility = JavaVersion.VERSION_11
apply from: "${rootDir}/../shared/examples_common.gradle"
def ROBOT_MAIN_CLASS = "frc.robot.Main"
// Define my targets (RoboRIO) and artifacts (deployable files)
// This is added by GradleRIO's backing project DeployUtils.
deploy {
targets {
roborio(getTargetTypeClass('RoboRIO')) {
// Team number is loaded either from the .wpilib/wpilib_preferences.json
// or from command line. If not found an exception will be thrown.
// You can use getTeamOrDefault(team) instead of getTeamNumber if you
// want to store a team number in this file.
team = project.frc.getTeamOrDefault(5940)
debug = project.frc.getDebugOrDefault(false)
artifacts {
// First part is artifact name, 2nd is artifact type
// getTargetTypeClass is a shortcut to get the class type using a string
frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
}
// Static files artifact
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/deploy'
}
}
}
}
}
def deployArtifact = deploy.targets.roborio.artifacts.frcJava
// Set to true to use debug for JNI.
wpi.java.debugJni = false
// Set this to true to enable desktop support.
def includeDesktopSupport = true
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
// Also defines JUnit 4.
dependencies {
implementation wpi.java.deps.wpilib()
implementation wpi.java.vendor.java()
roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
simulationDebug wpi.sim.enableDebug()
nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
simulationRelease wpi.sim.enableRelease()
testImplementation 'junit:junit:4.13.1'
}
// Simulation configuration (e.g. environment variables).
wpi.sim.addGui().defaultEnabled = true
wpi.sim.addDriverstation()
// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
}
// Configure jar and deploy tasks
deployArtifact.jarTask = jar
wpi.java.configureExecutableTasks(jar)
wpi.java.configureTestTasks(test)
// Configure string concat to always inline compile
tasks.withType(JavaCompile) {
options.compilerArgs.add '-XDstringConcat=inline'
}

104
photonlib-java-examples/apriltagExample/simgui-ds.json
View File

@@ -1,104 +0,0 @@
{
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"keyboardJoysticks": [
{
"axisConfig": [
{},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
},
{},
{
"decKey": 65,
"incKey": 68
}
],
"axisCount": 5,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard0"
}
]
}

63
photonlib-java-examples/apriltagExample/simgui.json
View File

@@ -1,63 +0,0 @@
{
"HALProvider": {
"Other Devices": {
"AnalogGyro[0]": {
"header": {
"open": true
}
}
}
},
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/LiveWindow/Ungrouped/AnalogGyro[0]": "Gyro",
"/LiveWindow/Ungrouped/MotorControllerGroup[1]": "Motor Controller",
"/LiveWindow/Ungrouped/MotorControllerGroup[2]": "Motor Controller",
"/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
"/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
"/SmartDashboard/Field": "Field2d",
"/SmartDashboard/VisionSystemSim-Test/Sim Field": "Field2d",
"/SmartDashboard/VisionSystemSim-YOUR CAMERA NAME/Sim Field": "Field2d"
},
"windows": {
"/SmartDashboard/Field": {
"window": {
"visible": true
}
},
"/SmartDashboard/VisionSystemSim-Test/Sim Field": {
"window": {
"visible": true
}
},
"/SmartDashboard/VisionSystemSim-YOUR CAMERA NAME/Sim Field": {
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"transitory": {
"LiveWindow": {
"open": true
},
"photonvision": {
"OV9281": {
"open": true
},
"USB_Camera": {
"open": true
},
"YOUR CAMERA NAME": {
"open": true
},
"open": true,
"testCamera": {
"open": true
}
}
}
}
}

57
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java
View File

@@ -1,57 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
public class Constants {
static class DriveTrainConstants {
static final double kMaxSpeed = 3.0; // meters per second
static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
static final double kTrackWidth = 0.381 * 2; // meters
static final double kWheelRadius = 0.0508; // meters
static final int kEncoderResolution = 4096;
static final double distancePerPulse = 2 * Math.PI * kWheelRadius / (double) kEncoderResolution;
}
static class FieldConstants {
static final double length = Units.feetToMeters(54);
static final double width = Units.feetToMeters(27);
}
static class VisionConstants {
static final Transform3d robotToCam =
new Transform3d(
new Translation3d(0.5, 0.0, 0.5),
new Rotation3d(
0, 0,
0)); // Cam mounted facing forward, half a meter forward of center, half a meter up
// from center.
static final String cameraName = "YOUR CAMERA NAME";
}
}

219
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java
View File

@@ -1,219 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Constants.DriveTrainConstants;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
/** Represents a differential drive style drivetrain. */
public class Drivetrain {
private final MotorController m_leftLeader = new PWMSparkMax(1);
private final MotorController m_leftFollower = new PWMSparkMax(2);
private final MotorController m_rightLeader = new PWMSparkMax(3);
private final MotorController m_rightFollower = new PWMSparkMax(4);
private final Encoder m_leftEncoder = new Encoder(0, 1);
private final Encoder m_rightEncoder = new Encoder(2, 3);
private final MotorControllerGroup m_leftGroup =
new MotorControllerGroup(m_leftLeader, m_leftFollower);
private final MotorControllerGroup m_rightGroup =
new MotorControllerGroup(m_rightLeader, m_rightFollower);
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(Constants.DriveTrainConstants.kTrackWidth);
public PhotonCameraWrapper pcw;
/*
* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry
* readings. The
* numbers used below are robot specific, and should be tuned.
*/
private final DifferentialDrivePoseEstimator m_poseEstimator =
new DifferentialDrivePoseEstimator(
m_kinematics, m_gyro.getRotation2d(), 0.0, 0.0, new Pose2d());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
// Simulation classes help us simulate our robot
private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
private final Field2d m_fieldSim = new Field2d();
private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.8);
private final DifferentialDrivetrainSim m_drivetrainSimulator =
new DifferentialDrivetrainSim(
m_drivetrainSystem,
DCMotor.getCIM(2),
8,
DriveTrainConstants.kTrackWidth,
DriveTrainConstants.kWheelRadius,
null);
// Simulated PhotonCamera -- only used in sim!
VisionSystemSim m_visionSystemSim;
/**
* Constructs a differential drive object. Sets the encoder distance per pulse and resets the
* gyro.
*/
public Drivetrain() {
pcw = new PhotonCameraWrapper();
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(DriveTrainConstants.distancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveTrainConstants.distancePerPulse);
m_leftEncoder.reset();
m_rightEncoder.reset();
SmartDashboard.putData("Field", m_fieldSim);
// Only simulate our PhotonCamera in simulation
if (RobotBase.isSimulation()) {
m_visionSystemSim = new VisionSystemSim(Constants.VisionConstants.cameraName);
var cameraSim =
new PhotonCameraSim(pcw.photonCamera, SimCameraProperties.PI4_LIFECAM_640_480());
m_visionSystemSim.addCamera(cameraSim, Constants.VisionConstants.robotToCam);
m_visionSystemSim.addAprilTags(pcw.photonPoseEstimator.getFieldTags());
}
}
/**
* Sets the desired wheel speeds.
*
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
m_leftGroup.setVoltage(leftOutput + leftFeedforward);
m_rightGroup.setVoltage(rightOutput + rightFeedforward);
}
/**
* Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity in m/s.
* @param rot Angular velocity in rad/s.
*/
public void drive(double xSpeed, double rot) {
var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
setSpeeds(wheelSpeeds);
}
/** Update our simulation. This should be run every robot loop in simulation. */
public void 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(
m_leftGroup.get() * RobotController.getInputVoltage(),
m_rightGroup.get() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
// Update results from vision as well
m_visionSystemSim.update(m_drivetrainSimulator.getPose());
}
/** Updates the field-relative position. */
public void updateOdometry() {
m_poseEstimator.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
Optional<EstimatedRobotPose> result =
pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition());
if (result.isPresent()) {
EstimatedRobotPose camPose = result.get();
m_poseEstimator.addVisionMeasurement(
camPose.estimatedPose.toPose2d(), camPose.timestampSeconds);
m_fieldSim.getObject("Cam Est Pos").setPose(camPose.estimatedPose.toPose2d());
} else {
// move it way off the screen to make it disappear
m_fieldSim.getObject("Cam Est Pos").setPose(new Pose2d(-100, -100, new Rotation2d()));
}
m_fieldSim.getObject("Actual Pos").setPose(m_drivetrainSimulator.getPose());
m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition());
}
}

76
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
View File

@@ -1,76 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import frc.robot.Constants.VisionConstants;
import java.io.IOException;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
public class PhotonCameraWrapper {
PhotonCamera photonCamera;
PhotonPoseEstimator photonPoseEstimator;
public PhotonCameraWrapper() {
// Change the name of your camera here to whatever it is in the PhotonVision UI.
photonCamera = new PhotonCamera(VisionConstants.cameraName);
try {
// Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field.
AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
// Create pose estimator
photonPoseEstimator =
new PhotonPoseEstimator(
fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
} catch (IOException e) {
// The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know
// where the tags are.
DriverStation.reportError("Failed to load AprilTagFieldLayout", e.getStackTrace());
photonPoseEstimator = null;
}
}
/**
* @param estimatedRobotPose The current best guess at robot pose
* @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create
* the estimate
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
if (photonPoseEstimator == null) {
// The field layout failed to load, so we cannot estimate poses.
return Optional.empty();
}
photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
return photonPoseEstimator.update();
}
}

101
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java
View File

@@ -1,101 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import frc.robot.Constants.DriveTrainConstants;
public class Robot extends TimedRobot {
private XboxController m_controller;
private Drivetrain m_drive;
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
public void robotInit() {
if (Robot.isSimulation()) {
NetworkTableInstance instance = NetworkTableInstance.getDefault();
// We have to have Photon running and set to NT server mode for it to connect
// to our computer instead of to a roboRIO.
instance.stopServer();
// set the NT server if simulating this code.
// "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor
instance.setServer("localhost");
instance.startClient4("Robot Simulation");
}
m_controller = new XboxController(0);
m_drive = new Drivetrain();
// Flush NetworkTables every loop. This ensures that robot pose and other values
// are sent during every iteration. This only applies to local NT connections!
setNetworkTablesFlushEnabled(true);
}
@Override
public void robotPeriodic() {
m_drive.updateOdometry();
}
@Override
public void autonomousInit() {}
@Override
public void autonomousPeriodic() {}
@Override
public void teleopPeriodic() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
var joyY = m_controller.getLeftY();
if (Math.abs(joyY) < 0.075) {
joyY = 0;
}
final var xSpeed = -m_speedLimiter.calculate(joyY) * DriveTrainConstants.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.
var joyX = m_controller.getRightX();
if (Math.abs(joyX) < 0.075) {
joyX = 0;
}
final var rot = -m_rotLimiter.calculate(joyX) * DriveTrainConstants.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
}
@Override
public void simulationPeriodic() {
m_drive.simulationPeriodic();
}
}

4
photonlib-java-examples/build.gradle
View File

@@ -2,6 +2,8 @@ plugins {
id "com.diffplug.spotless" version "6.1.2" id "com.diffplug.spotless" version "6.1.2"
} }
apply from: "examples.gradle"
allprojects { allprojects {
repositories { repositories {
mavenCentral() mavenCentral()
@@ -29,7 +31,7 @@ spotless {
// Task that depends on the build task for every example // Task that depends on the build task for every example
task buildAllExamples { task -> task buildAllExamples { task ->
new File('examples.txt').eachLine { line -> exampleFolderNames.each { line ->
task.dependsOn(line + ":build") task.dependsOn(line + ":build")
} }
} }

14
photonlib-java-examples/examples.gradle Normal file
View File

@@ -0,0 +1,14 @@
// These should be the only 2 non-project subdirectories in the examples folder
// I could check for (it)/build.gradle to exist, but w/e
def EXCLUDED_DIRS = ["bin", "build"]
// List all non-hidden directories not in EXCUDED_DIRS
ext.exampleFolderNames = file("${rootDir}")
.listFiles()
.findAll {
return (it.isDirectory()
&& !it.isHidden()
&& !(it.name in EXCLUDED_DIRS) && !it.name.startsWith(".")
&& it.toPath().resolve("build.gradle").toFile().exists())
}
.collect { it.name }

6
photonlib-java-examples/examples.txt
View File

@@ -1,6 +0,0 @@
aimandrange
aimattarget
getinrange
simaimandrange
simposeest
apriltagExample

3
photonlib-java-examples/getinrange/README.md Normal file
View File

@@ -0,0 +1,3 @@
## **`getinrange`**
### See [PhotonLib Java Examples](./README.md#getinrange)

7
photonlib-java-examples/settings.gradle
View File

@@ -1,6 +1,3 @@
include 'photon-targeting' apply from: "examples.gradle"
include 'photon-core'
include 'photon-server'
include 'photon-lib'
new File('examples.txt').eachLine { line -> include line } exampleFolderNames.each { line -> include line }

1
photonlib-java-examples/simaimandrange/.gitignore vendored
View File

@@ -160,3 +160,4 @@ bin/
# Simulation GUI and other tools window save file # Simulation GUI and other tools window save file
*-window.json *-window.json
networktables.json

3
photonlib-java-examples/simaimandrange/README.md Normal file
View File

@@ -0,0 +1,3 @@
## **`simaimandrange`**
### See [PhotonLib Java Examples](./README.md#simaimandrange)

25
photonlib-java-examples/simaimandrange/simgui-ds.json
View File

@@ -1,28 +1,25 @@
{ {
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"keyboardJoysticks": [ "keyboardJoysticks": [
{ {
"axisConfig": [ "axisConfig": [
{},
{ {
"decKey": 87, "decKey": 87,
"incKey": 83 "incKey": 83
}, },
{},
{ {
"decKey": 69, "decayRate": 0.10000000149011612,
"decayRate": 0.0, "incKey": 81,
"incKey": 82, "keyRate": 0.10000000149011612
"keyRate": 0.009999999776482582
}, },
{},
{},
{ {
"decKey": 68, "decayRate": 0.10000000149011612,
"incKey": 65 "incKey": 69,
"keyRate": 0.10000000149011612
},
{
"decKey": 65,
"incKey": 68
} }
], ],
"axisCount": 6, "axisCount": 6,

56
photonlib-java-examples/simaimandrange/simgui.json
View File

@@ -2,16 +2,47 @@
"NTProvider": { "NTProvider": {
"types": { "types": {
"/FMSInfo": "FMSInfo", "/FMSInfo": "FMSInfo",
"/SmartDashboard/Field": "Field2d", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
"/SmartDashboard/photonvision Sim Field": "Field2d"
}, },
"windows": { "windows": {
"/SmartDashboard/Field": { "/SmartDashboard/VisionSystemSim-main/Sim Field": {
"window": { "Robot": {
"visible": true "arrowColor": [
} 1.0,
}, 1.0,
"/SmartDashboard/photonvision Sim Field": { 1.0,
255.0
],
"arrowWeight": 3.0,
"color": [
1.0,
1.0,
1.0,
255.0
],
"weight": 3.0
},
"cameras": {
"arrowColor": [
0.0,
0.683544397354126,
1.0,
255.0
],
"arrowSize": 29,
"selectable": false,
"style": "Hidden"
},
"targets": {
"color": [
0.05063295364379883,
1.0,
0.0,
255.0
],
"style": "Hidden",
"weight": 1.0
},
"window": { "window": {
"visible": true "visible": true
} }
@@ -21,7 +52,7 @@
"NetworkTables": { "NetworkTables": {
"transitory": { "transitory": {
"SmartDashboard": { "SmartDashboard": {
"Field": { "VisionSystemSim-main/Sim Field": {
"open": true "open": true
}, },
"open": true "open": true
@@ -33,12 +64,5 @@
} }
} }
} }
},
"Plot": {
"Plot <0>": {
"window": {
"visible": false
}
}
} }
} }

20
photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java
View File

@@ -56,8 +56,8 @@ public class Robot extends TimedRobot {
PhotonCamera camera = new PhotonCamera("photonvision"); PhotonCamera camera = new PhotonCamera("photonvision");
// PID constants should be tuned per robot // PID constants should be tuned per robot
final double LINEAR_P = 2.0; final double LINEAR_P = 0.5;
final double LINEAR_D = 0.0; final double LINEAR_D = 0.1;
PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
final double ANGULAR_P = 0.03; final double ANGULAR_P = 0.03;
@@ -71,6 +71,12 @@ public class Robot extends TimedRobot {
PWMVictorSPX rightMotor = new PWMVictorSPX(1); PWMVictorSPX rightMotor = new PWMVictorSPX(1);
DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
@Override
public void robotInit() {
leftMotor.setInverted(false);
rightMotor.setInverted(true);
}
@Override @Override
public void teleopPeriodic() { public void teleopPeriodic() {
double forwardSpeed; double forwardSpeed;
@@ -91,11 +97,11 @@ public class Robot extends TimedRobot {
Units.degreesToRadians(result.getBestTarget().getPitch())); Units.degreesToRadians(result.getBestTarget().getPitch()));
// Use this range as the measurement we give to the PID controller. // Use this range as the measurement we give to the PID controller.
// -1.0 required to ensure positive PID controller effort _increases_ range // (This forwardSpeed must be positive to go forward.)
forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS); forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS);
// Also calculate angular power // Also calculate angular power
// -1.0 required to ensure positive PID controller effort _increases_ yaw // (This rotationSpeed must be positive to turn counter-clockwise.)
rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0); rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0);
} else { } else {
// If we have no targets, stay still. // If we have no targets, stay still.
@@ -104,8 +110,8 @@ public class Robot extends TimedRobot {
} }
} else { } else {
// Manual Driver Mode // Manual Driver Mode
forwardSpeed = -xboxController.getRightY(); forwardSpeed = -xboxController.getLeftY() * 0.75;
rotationSpeed = xboxController.getLeftX(); rotationSpeed = -xboxController.getRightX() * 0.75;
} }
// Use our forward/turn speeds to control the drivetrain // Use our forward/turn speeds to control the drivetrain
@@ -119,7 +125,7 @@ public class Robot extends TimedRobot {
@Override @Override
public void simulationInit() { public void simulationInit() {
dtSim = new DrivetrainSim(); dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera);
} }
@Override @Override

91
photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java
View File

@@ -26,6 +26,7 @@ package frc.robot.sim;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Translation3d;
@@ -38,11 +39,14 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.PWMSim; import edu.wpi.first.wpilibj.simulation.PWMSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot; import frc.robot.Robot;
import org.photonvision.simulation.SimVisionSystem; import java.util.List;
import org.photonvision.simulation.SimVisionTarget; import org.photonvision.PhotonCamera;
import org.photonvision.estimation.TargetModel;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.simulation.VisionTargetSim;
/** /**
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
@@ -53,53 +57,48 @@ import org.photonvision.simulation.SimVisionTarget;
*/ */
public class DrivetrainSim { public class DrivetrainSim {
// Simulated Motor Controllers // Simulated Motor Controllers
PWMSim leftLeader = new PWMSim(0); PWMSim leftLeader;
PWMSim rightLeader = new PWMSim(1); PWMSim rightLeader;
// Simulation Physics // Simulation Physics
// Configure these to match your drivetrain's physical dimensions // Configure these to match your drivetrain's physical dimensions
// and characterization results. // and characterization results.
double trackwidthMeters = Units.feetToMeters(2.0);
LinearSystem<N2, N2, N2> drivetrainSystem = LinearSystem<N2, N2, N2> drivetrainSystem =
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3, 1.0); LinearSystemId.identifyDrivetrainSystem(2.0, 0.5, 2.25, 0.3, trackwidthMeters);
DifferentialDrivetrainSim drivetrainSimulator = DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim( new DifferentialDrivetrainSim(
drivetrainSystem, drivetrainSystem,
DCMotor.getCIM(2), DCMotor.getCIM(2),
8, 8,
Units.feetToMeters(2.0), trackwidthMeters,
Units.inchesToMeters(6.0 / 2.0), Units.inchesToMeters(6.0 / 2.0),
null); null);
// Simulated Vision System. // Simulated Vision System.
// Configure these to match your PhotonVision Camera, // Configure these to match your PhotonVision Camera,
// pipeline, and LED setup. // pipeline, and LED setup.
double camDiagFOV = 170.0; // degrees - assume wide-angle camera double camDiagFOV = 100.0; // degrees
double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees
double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
double minTargetArea = 0.1; // percentage (0 - 100)
double maxLEDRange = 20; // meters double maxLEDRange = 20; // meters
int camResolutionWidth = 640; // pixels int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels int camResolutionHeight = 480; // pixels
double minTargetArea = 10; // square pixels PhotonCameraSim cameraSim;
SimVisionSystem simVision = VisionSystemSim visionSim = new VisionSystemSim("main");
new SimVisionSystem(
"photonvision",
camDiagFOV,
new Transform3d(
new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)),
maxLEDRange,
camResolutionWidth,
camResolutionHeight,
minTargetArea);
// See // See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 208 // page 208
double targetWidth = Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters TargetModel targetModel =
// See new TargetModel(
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf List.of(
// page 197 new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)),
double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)),
new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)),
new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5))));
// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
// pages 4 and 5 // pages 4 and 5
double tgtXPos = Units.feetToMeters(54); double tgtXPos = Units.feetToMeters(54);
@@ -108,13 +107,36 @@ public class DrivetrainSim {
Pose3d farTargetPose = Pose3d farTargetPose =
new Pose3d( new Pose3d(
new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS), new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
new Rotation3d(0.0, 0.0, 0.0)); new Rotation3d(0.0, 0.0, Math.PI));
Field2d field = new Field2d(); public DrivetrainSim(int leftMotorPort, int rightMotorPort, PhotonCamera camera) {
leftLeader = new PWMSim(leftMotorPort);
rightLeader = new PWMSim(rightMotorPort);
public DrivetrainSim() { // Make the vision target visible to this simulated field.
simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1)); var visionTarget = new VisionTargetSim(farTargetPose, targetModel);
SmartDashboard.putData("Field", field); visionSim.addVisionTargets(visionTarget);
// Create simulated camera properties. These can be set to mimic your actual camera.
var cameraProp = new SimCameraProperties();
cameraProp.setCalibration(
camResolutionWidth, camResolutionHeight, Rotation2d.fromDegrees(camDiagFOV));
cameraProp.setCalibError(0.2, 0.05);
cameraProp.setFPS(25);
cameraProp.setAvgLatencyMs(30);
cameraProp.setLatencyStdDevMs(4);
// Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible
// targets.
cameraSim = new PhotonCameraSim(camera, cameraProp, minTargetArea, maxLEDRange);
// Add the simulated camera to view the targets on this simulated field.
visionSim.addCamera(
cameraSim,
new Transform3d(
new Translation3d(0.25, 0, Robot.CAMERA_HEIGHT_METERS),
new Rotation3d(0, -Robot.CAMERA_PITCH_RADIANS, 0)));
cameraSim.enableDrawWireframe(true);
} }
/** /**
@@ -131,14 +153,12 @@ public class DrivetrainSim {
} }
drivetrainSimulator.setInputs( drivetrainSimulator.setInputs(
leftMotorCmd * RobotController.getInputVoltage(), leftMotorCmd * RobotController.getBatteryVoltage(),
-rightMotorCmd * RobotController.getInputVoltage()); -rightMotorCmd * RobotController.getBatteryVoltage());
drivetrainSimulator.update(0.02); drivetrainSimulator.update(0.02);
// Update PhotonVision based on our new robot position. // Update PhotonVision based on our new robot position.
simVision.processFrame(drivetrainSimulator.getPose()); visionSim.update(drivetrainSimulator.getPose());
field.setRobotPose(drivetrainSimulator.getPose());
} }
/** /**
@@ -149,5 +169,6 @@ public class DrivetrainSim {
*/ */
public void resetPose(Pose2d pose) { public void resetPose(Pose2d pose) {
drivetrainSimulator.setPose(pose); drivetrainSimulator.setPose(pose);
visionSim.resetRobotPose(pose);
} }
} }

162
photonlib-java-examples/simposeest/.gitignore vendored
View File

@@ -1,162 +0,0 @@
# This gitignore has been specially created by the WPILib team.
# If you remove items from this file, intellisense might break.
### C++ ###
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
### Java ###
# Compiled class file
*.class
# Log file
*.log
# BlueJ files
*.ctxt
# Mobile Tools for Java (J2ME)
.mtj.tmp/
# Package Files #
*.jar
*.war
*.nar
*.ear
*.zip
*.tar.gz
*.rar
# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
hs_err_pid*
### Linux ###
*~
# temporary files which can be created if a process still has a handle open of a deleted file
.fuse_hidden*
# KDE directory preferences
.directory
# Linux trash folder which might appear on any partition or disk
.Trash-*
# .nfs files are created when an open file is removed but is still being accessed
.nfs*
### macOS ###
# General
.DS_Store
.AppleDouble
.LSOverride
# Icon must end with two \r
Icon
# Thumbnails
._*
# Files that might appear in the root of a volume
.DocumentRevisions-V100
.fseventsd
.Spotlight-V100
.TemporaryItems
.Trashes
.VolumeIcon.icns
.com.apple.timemachine.donotpresent
# Directories potentially created on remote AFP share
.AppleDB
.AppleDesktop
Network Trash Folder
Temporary Items
.apdisk
### VisualStudioCode ###
.vscode/*
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
!.vscode/extensions.json
### Windows ###
# Windows thumbnail cache files
Thumbs.db
ehthumbs.db
ehthumbs_vista.db
# Dump file
*.stackdump
# Folder config file
[Dd]esktop.ini
# Recycle Bin used on file shares
$RECYCLE.BIN/
# Windows Installer files
*.cab
*.msi
*.msix
*.msm
*.msp
# Windows shortcuts
*.lnk
### Gradle ###
.gradle
/build/
# Ignore Gradle GUI config
gradle-app.setting
# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
!gradle-wrapper.jar
# Cache of project
.gradletasknamecache
# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
# gradle/wrapper/gradle-wrapper.properties
# # VS Code Specific Java Settings
# DO NOT REMOVE .classpath and .project
.classpath
.project
.settings/
bin/
# Simulation GUI and other tools window save file
*-window.json

24
photonlib-java-examples/simposeest/WPILib-License.md
View File

@@ -1,24 +0,0 @@
Copyright (c) 2009-2021 FIRST and other WPILib contributors
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

29
photonlib-java-examples/simposeest/settings.gradle
View File

@@ -1,29 +0,0 @@
import org.gradle.internal.os.OperatingSystem
rootProject.name = 'simposeest'
pluginManagement {
repositories {
mavenLocal()
gradlePluginPortal()
String frcYear = '2023'
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
}
}
}

57
photonlib-java-examples/simposeest/simgui.json
View File

@@ -1,57 +0,0 @@
{
"HALProvider": {
"Other Devices": {
"AnalogGyro[0]": {
"header": {
"open": true
}
}
}
},
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/SmartDashboard/Field": "Field2d",
"/SmartDashboard/mainCam Sim Field": "Field2d"
},
"windows": {
"/SmartDashboard/Field": {
"window": {
"visible": true
}
},
"/SmartDashboard/mainCam Sim Field": {
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"transitory": {
"SmartDashboard": {
"Field": {
"open": true
},
"open": true
}
}
},
"NetworkTables Info": {
"Clients": {
"open": true
},
"Connections": {
"open": true
},
"Server": {
"Publishers": {
"open": true
},
"Subscribers": {
"open": true
},
"open": true
}
}
}

3
photonlib-java-examples/simposeest/src/main/deploy/example.txt
View File

@@ -1,3 +0,0 @@
Files placed in this directory will be deployed to the RoboRIO into the
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
to get a proper path relative to the deploy directory.

110
photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java
View File

@@ -1,110 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.Timer;
import java.util.List;
/**
* Implements logic to convert a set of desired waypoints (ie, a trajectory) and the current
* estimate of where the robot is at (ie, the estimated Pose) into motion commands for a drivetrain.
* The Ramaste controller is used to smoothly move the robot from where it thinks it is to where it
* thinks it ought to be.
*/
public class AutoController {
private Trajectory trajectory;
private RamseteController ramsete = new RamseteController();
private Timer timer = new Timer();
boolean isRunning = false;
Trajectory.State desiredDtState;
public AutoController() {
// Change this trajectory if you need the robot to follow different paths.
trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(2, 2, new Rotation2d()),
List.of(),
new Pose2d(6, 4, new Rotation2d()),
new TrajectoryConfig(2, 2));
}
/**
* @return The starting (initial) pose of the currently-active trajectory
*/
public Pose2d getInitialPose() {
return trajectory.getInitialPose();
}
/** Starts the controller running. Call this at the start of autonomous */
public void startPath() {
timer.reset();
timer.start();
isRunning = true;
}
/** Stops the controller from generating commands */
public void stopPath() {
isRunning = false;
timer.reset();
}
/**
* Given the current estimate of the robot's position, calculate drivetrain speed commands which
* will best-execute the active trajectory. Be sure to call `startPath()` prior to calling this
* method.
*
* @param curEstPose Current estimate of drivetrain pose on the field
* @return The commanded drivetrain motion
*/
public ChassisSpeeds getCurMotorCmds(Pose2d curEstPose) {
if (isRunning) {
double elapsed = timer.get();
desiredDtState = trajectory.sample(elapsed);
} else {
desiredDtState = new Trajectory.State();
}
return ramsete.calculate(curEstPose, desiredDtState);
}
/**
* @return The position which the auto controller is attempting to move the drivetrain to right
* now.
*/
public Pose2d getCurPose2d() {
return desiredDtState.poseMeters;
}
}

110
photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java
View File

@@ -1,110 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.util.Units;
import org.photonvision.simulation.SimVisionTarget;
/**
* Holding class for all physical constants that must be used throughout the codebase. These values
* should be set by one of a few methods: 1) Talk to your mechanical and electrical teams and
* determine how the physical robot is being built and configured. 2) Read the game manual and look
* at the field drawings 3) Match with how your vision coprocessor is configured.
*/
public class Constants {
//////////////////////////////////////////////////////////////////
// Drivetrain Physical
//////////////////////////////////////////////////////////////////
public static final double kMaxSpeed = 3.0; // 3 meters per second.
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second.
public static final double kTrackWidth = 0.381 * 2;
public static final double kWheelRadius = 0.0508;
public static final int kEncoderResolution = 4096;
public static final DifferentialDriveKinematics kDtKinematics =
new DifferentialDriveKinematics(kTrackWidth);
//////////////////////////////////////////////////////////////////
// Electrical IO
//////////////////////////////////////////////////////////////////
public static final int kGyroPin = 0;
public static final int kDtLeftEncoderPinA = 0;
public static final int kDtLeftEncoderPinB = 1;
public static final int kDtRightEncoderPinA = 2;
public static final int kDtRightEncoderPinB = 3;
public static final int kDtLeftLeaderPin = 1;
public static final int kDtLeftFollowerPin = 2;
public static final int kDtRightLeaderPin = 3;
public static final int kDtRightFollowerPin = 4;
//////////////////////////////////////////////////////////////////
// Vision Processing
//////////////////////////////////////////////////////////////////
// Name configured in the PhotonVision GUI for the camera
public static final String kCamName = "mainCam";
// Physical location of the camera on the robot, relative to the center of the
// robot.
public static final Transform3d kCameraToRobot =
new Transform3d(
new Translation3d(-0.25, 0, -.25), // in meters
new Rotation3d());
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 208
public static final double targetWidth =
Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 197
public static final double targetHeight =
Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
// pages 4 and 5
public static final double kFarTgtXPos = Units.feetToMeters(54);
public static final double kFarTgtYPos =
Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
public static final double kFarTgtZPos =
(Units.inchesToMeters(98.19) - targetHeight) / 2 + targetHeight;
public static final Pose3d kFarTargetPose =
new Pose3d(
new Translation3d(kFarTgtXPos, kFarTgtYPos, kFarTgtZPos),
new Rotation3d(0.0, 0.0, Units.degreesToRadians(180)));
public static final SimVisionTarget kFarTarget =
new SimVisionTarget(kFarTargetPose, targetWidth, targetHeight, 42);
}

139
photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java
View File

@@ -1,139 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
/**
* Implements a controller for the drivetrain. Converts a set of chassis motion commands into motor
* controller PWM values which attempt to speed up or slow down the wheels to match the desired
* speed.
*/
public class Drivetrain {
// PWM motor controller output definitions
PWMVictorSPX leftLeader = new PWMVictorSPX(Constants.kDtLeftLeaderPin);
PWMVictorSPX leftFollower = new PWMVictorSPX(Constants.kDtLeftFollowerPin);
PWMVictorSPX rightLeader = new PWMVictorSPX(Constants.kDtRightLeaderPin);
PWMVictorSPX rightFollower = new PWMVictorSPX(Constants.kDtRightFollowerPin);
MotorControllerGroup leftGroup = new MotorControllerGroup(leftLeader, leftFollower);
MotorControllerGroup rightGroup = new MotorControllerGroup(rightLeader, rightFollower);
// Drivetrain wheel speed sensors
// Used both for speed control and pose estimation.
Encoder leftEncoder = new Encoder(Constants.kDtLeftEncoderPinA, Constants.kDtLeftEncoderPinB);
Encoder rightEncoder = new Encoder(Constants.kDtRightEncoderPinA, Constants.kDtRightEncoderPinB);
// Drivetrain Pose Estimation
final DrivetrainPoseEstimator poseEst;
// Kinematics - defines the physical size and shape of the drivetrain, which is
// required to convert from
// chassis speed commands to wheel speed commands.
DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Constants.kTrackWidth);
// Closed-loop PIDF controllers for servoing each side of the drivetrain to a
// specific speed.
// Gains are for example purposes only - must be determined for your own robot!
SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(1, 3);
PIDController leftPIDController = new PIDController(8.5, 0, 0);
PIDController rightPIDController = new PIDController(8.5, 0, 0);
public Drivetrain() {
// 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.
leftEncoder.setDistancePerPulse(
2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution);
rightEncoder.setDistancePerPulse(
2 * Math.PI * Constants.kWheelRadius / Constants.kEncoderResolution);
leftEncoder.reset();
rightEncoder.reset();
rightGroup.setInverted(true);
poseEst = new DrivetrainPoseEstimator(leftEncoder.getDistance(), rightEncoder.getDistance());
}
/**
* Given a set of chassis (fwd/rev + rotate) speed commands, perform all periodic tasks to assign
* new outputs to the motor controllers.
*
* @param xSpeed Desired chassis Forward or Reverse speed (in meters/sec). Positive is forward.
* @param rot Desired chassis rotation speed in radians/sec. Positive is counter-clockwise.
*/
public void drive(double xSpeed, double rot) {
// Convert our fwd/rev and rotate commands to wheel speed commands
DifferentialDriveWheelSpeeds speeds =
kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot));
// Calculate the feedback (PID) portion of our motor command, based on desired
// wheel speed
var leftOutput = leftPIDController.calculate(leftEncoder.getRate(), speeds.leftMetersPerSecond);
var rightOutput =
rightPIDController.calculate(rightEncoder.getRate(), speeds.rightMetersPerSecond);
// Calculate the feedforward (F) portion of our motor command, based on desired
// wheel speed
var leftFeedforward = feedforward.calculate(speeds.leftMetersPerSecond);
var rightFeedforward = feedforward.calculate(speeds.rightMetersPerSecond);
// Update the motor controllers with our new motor commands
leftGroup.setVoltage(leftOutput + leftFeedforward);
rightGroup.setVoltage(rightOutput + rightFeedforward);
// Update the pose estimator with the most recent sensor readings.
poseEst.update(leftEncoder.getDistance(), rightEncoder.getDistance());
}
/**
* Force the pose estimator and all sensors to a particular pose. This is useful for indicating to
* the software when you have manually moved your robot in a particular position on the field (EX:
* when you place it on the field at the start of the match).
*
* @param pose
*/
public void resetOdometry(Pose2d pose) {
leftEncoder.reset();
rightEncoder.reset();
poseEst.resetToPose(pose, leftEncoder.getDistance(), rightEncoder.getDistance());
}
/**
* @return The current best-guess at drivetrain Pose on the field.
*/
public Pose2d getCtrlsPoseEstimate() {
return poseEst.getPoseEst();
}
}

109
photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java
View File

@@ -1,109 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.AnalogGyro;
import org.photonvision.PhotonCamera;
/**
* Performs estimation of the drivetrain's current position on the field, using a vision system,
* drivetrain encoders, and a gyroscope. These sensor readings are fused together using a Kalman
* filter. This in turn creates a best-guess at a Pose2d of where our drivetrain is currently at.
*/
public class DrivetrainPoseEstimator {
// Sensors used as part of the Pose Estimation
private final AnalogGyro gyro = new AnalogGyro(Constants.kGyroPin);
private PhotonCamera cam = new PhotonCamera(Constants.kCamName);
// Note - drivetrain encoders are also used. The Drivetrain class must pass us
// the relevant readings.
// Kalman Filter Configuration. These can be "tuned-to-taste" based on how much
// you trust your
// various sensors. Smaller numbers will cause the filter to "trust" the
// estimate from that particular
// component more than the others. This in turn means the particualr component
// will have a stronger
// influence on the final pose estimate.
Matrix<N5, N1> stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05);
Matrix<N3, N1> localMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1));
Matrix<N3, N1> visionMeasurementStdDevs =
VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1));
private final DifferentialDrivePoseEstimator m_poseEstimator;
public DrivetrainPoseEstimator(double leftDist, double rightDist) {
m_poseEstimator =
new DifferentialDrivePoseEstimator(
Constants.kDtKinematics,
gyro.getRotation2d(),
0, // Assume zero encoder counts at start
0,
new Pose2d()); // Default - start at origin. This will get reset by the autonomous init
}
/**
* Perform all periodic pose estimation tasks.
*
* @param actWheelSpeeds Current Speeds (in m/s) of the drivetrain wheels
* @param leftDist Distance (in m) the left wheel has traveled
* @param rightDist Distance (in m) the right wheel has traveled
*/
public void update(double leftDist, double rightDist) {
m_poseEstimator.update(gyro.getRotation2d(), leftDist, rightDist);
var res = cam.getLatestResult();
if (res.hasTargets()) {
var imageCaptureTime = res.getTimestampSeconds();
var camToTargetTrans = res.getBestTarget().getBestCameraToTarget();
var camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
m_poseEstimator.addVisionMeasurement(
camPose.transformBy(Constants.kCameraToRobot).toPose2d(), imageCaptureTime);
}
}
/**
* Force the pose estimator to a particular pose. This is useful for indicating to the software
* when you have manually moved your robot in a particular position on the field (EX: when you
* place it on the field at the start of the match).
*/
public void resetToPose(Pose2d pose, double leftDist, double rightDist) {
m_poseEstimator.resetPosition(gyro.getRotation2d(), leftDist, rightDist, pose);
}
/**
* @return The current best-guess at drivetrain position on the field.
*/
public Pose2d getPoseEst() {
return m_poseEstimator.getEstimatedPosition();
}
}

172
photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java
View File

@@ -1,172 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.simulation.PWMSim;
import org.photonvision.simulation.SimVisionSystem;
/**
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
* PhotonVision system and one vision target.
*
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
* real motors/sensors/physics are used instead.
*/
public class DrivetrainSim {
// Simulated Sensors
AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
EncoderSim leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA);
EncoderSim rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA);
// Simulated Motor Controllers
PWMSim leftLeader = new PWMSim(Constants.kDtLeftLeaderPin);
PWMSim leftFollower = new PWMSim(Constants.kDtLeftFollowerPin);
PWMSim rightLeader = new PWMSim(Constants.kDtRightLeaderPin);
PWMSim rightFollower = new PWMSim(Constants.kDtRightFollowerPin);
// Simulation Physics
// Configure these to match your drivetrain's physical dimensions
// and characterization results.
LinearSystem<N2, N2, N2> drivetrainSystem =
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim(
drivetrainSystem,
DCMotor.getCIM(2),
8,
Constants.kTrackWidth,
Constants.kWheelRadius,
null);
// Simulated Vision System.
// Configure these to match your PhotonVision Camera,
// pipeline, and LED setup.
double camDiagFOV = 75.0; // degrees
double camPitch = 15.0; // degrees
double camHeightOffGround = 0.85; // meters
double maxLEDRange = 20; // meters
int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels
double minTargetArea = 10; // square pixels
SimVisionSystem simVision =
new SimVisionSystem(
Constants.kCamName,
camDiagFOV,
Constants.kCameraToRobot,
maxLEDRange,
camResolutionWidth,
camResolutionHeight,
minTargetArea);
public DrivetrainSim() {
simVision.addSimVisionTarget(Constants.kFarTarget);
}
/**
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
* physics forward by a single 20ms step.
*/
public void update() {
double leftMotorCmd = 0;
double rightMotorCmd = 0;
if (DriverStation.isEnabled() && !RobotController.isBrownedOut()) {
// If the motor controllers are enabled...
// Roughly model the effect of leader and follower motor pushing on the same
// gearbox.
// Note if the software is incorrect and drives them against each other, speed
// will be
// roughly matching the physical situation, but current draw will _not_ be
// accurate.
leftMotorCmd = (leftLeader.getSpeed() + leftFollower.getSpeed()) / 2.0;
rightMotorCmd = (rightLeader.getSpeed() + rightFollower.getSpeed()) / 2.0;
}
// Update the physics simulation
drivetrainSimulator.setInputs(
leftMotorCmd * RobotController.getInputVoltage(),
-rightMotorCmd * RobotController.getInputVoltage());
drivetrainSimulator.update(0.02);
// Update our sensors based on the simulated motion of the robot
leftEncoderSim.setDistance((drivetrainSimulator.getLeftPositionMeters()));
leftEncoderSim.setRate((drivetrainSimulator.getLeftVelocityMetersPerSecond()));
rightEncoderSim.setDistance((drivetrainSimulator.getRightPositionMeters()));
rightEncoderSim.setRate((drivetrainSimulator.getRightVelocityMetersPerSecond()));
gyroSim.setAngle(
-drivetrainSimulator
.getHeading()
.getDegrees()); // Gyros have an inverted reference frame for
// angles, so multiply by -1.0;
// Update PhotonVision based on our new robot position.
simVision.processFrame(drivetrainSimulator.getPose());
}
/**
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
* robot onto a specific spot in the field (IE, at the start of each match).
*
* @param pose
*/
public void resetPose(Pose2d pose) {
drivetrainSimulator.setPose(pose);
}
/**
* @return The simulated robot's position, in meters.
*/
public Pose2d getCurPose() {
return drivetrainSimulator.getPose();
}
/**
* For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if
* you magically kicked your robot to the side in a way the encoders and gyro didn't measure.
*
* <p>This distrubance should be corrected for once a vision target is in view.
*/
public void applyKick() {
Pose2d newPose =
drivetrainSimulator
.getPose()
.transformBy(new Transform2d(new Translation2d(0, 0.5), new Rotation2d()));
drivetrainSimulator.setPose(newPose);
}
}

45
photonlib-java-examples/simposeest/src/main/java/frc/robot/Main.java
View File

@@ -1,45 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

57
photonlib-java-examples/simposeest/src/main/java/frc/robot/OperatorInterface.java
View File

@@ -1,57 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.wpilibj.XboxController;
public class OperatorInterface {
private XboxController opCtrl = new XboxController(0);
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
private SlewRateLimiter speedLimiter = new SlewRateLimiter(3);
private SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
public OperatorInterface() {}
public double getFwdRevSpdCmd() {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
return -speedLimiter.calculate(opCtrl.getLeftY()) * Constants.kMaxSpeed;
}
public double getRotateSpdCmd() {
// 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.
return -rotLimiter.calculate(opCtrl.getRightX()) * Constants.kMaxAngularSpeed;
}
public boolean getSimKickCmd() {
return opCtrl.getXButtonPressed();
}
}

61
photonlib-java-examples/simposeest/src/main/java/frc/robot/PoseTelemetry.java
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.
*/
package frc.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/** Reports our expected, desired, and actual poses to dashboards */
public class PoseTelemetry {
Field2d field = new Field2d();
Pose2d actPose = new Pose2d();
Pose2d desPose = new Pose2d();
Pose2d estPose = new Pose2d();
public PoseTelemetry() {
SmartDashboard.putData("Field", field);
update();
}
public void update() {
field.getObject("DesPose").setPose(desPose);
field.getObject("ActPose").setPose(actPose);
field.getObject("Robot").setPose(estPose);
}
public void setActualPose(Pose2d in) {
actPose = in;
}
public void setDesiredPose(Pose2d in) {
desPose = in;
}
public void setEstimatedPose(Pose2d in) {
estPose = in;
}
}

90
photonlib-java-examples/simposeest/src/main/java/frc/robot/Robot.java
View File

@@ -1,90 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.TimedRobot;
public class Robot extends TimedRobot {
AutoController autoCtrl = new AutoController();
Drivetrain dt = new Drivetrain();
OperatorInterface opInf = new OperatorInterface();
DrivetrainSim dtSim = new DrivetrainSim();
PoseTelemetry pt = new PoseTelemetry();
@Override
public void robotInit() {
// Flush NetworkTables every loop. This ensures that robot pose and other values
// are sent during every iteration.
setNetworkTablesFlushEnabled(true);
}
@Override
public void autonomousInit() {
resetOdometery();
autoCtrl.startPath();
}
@Override
public void autonomousPeriodic() {
ChassisSpeeds speeds = autoCtrl.getCurMotorCmds(dt.getCtrlsPoseEstimate());
dt.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
pt.setDesiredPose(autoCtrl.getCurPose2d());
}
@Override
public void teleopPeriodic() {
dt.drive(opInf.getFwdRevSpdCmd(), opInf.getRotateSpdCmd());
}
@Override
public void robotPeriodic() {
pt.setEstimatedPose(dt.getCtrlsPoseEstimate());
pt.update();
}
@Override
public void disabledPeriodic() {
dt.drive(0, 0);
}
@Override
public void simulationPeriodic() {
if (opInf.getSimKickCmd()) {
dtSim.applyKick();
}
dtSim.update();
pt.setActualPose(dtSim.getCurPose());
}
private void resetOdometery() {
Pose2d startPose = autoCtrl.getInitialPose();
dtSim.resetPose(startPose);
dt.resetOdometry(startPose);
}
}

11
photonlib-java-examples/apriltagExample/.gitignore → photonlib-java-examples/swervedriveposeestsim/.gitignore vendored
View File

@@ -158,5 +158,16 @@ gradle-app.setting
.settings/ .settings/
bin/ bin/
# IntelliJ
*.iml
*.ipr
*.iws
.idea/
out/
# Fleet
.fleet
# Simulation GUI and other tools window save file # Simulation GUI and other tools window save file
*-window.json *-window.json
networktables.json

2
photonlib-java-examples/simposeest/.wpilib/wpilib_preferences.json → photonlib-java-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json
View File

@@ -2,5 +2,5 @@
"enableCppIntellisense": false, "enableCppIntellisense": false,
"currentLanguage": "java", "currentLanguage": "java",
"projectYear": "2023", "projectYear": "2023",
"teamNumber": 6995 "teamNumber": 4512
} }

3
photonlib-java-examples/swervedriveposeestsim/README.md Normal file
View File

@@ -0,0 +1,3 @@
## **`swervedriveposeestsim`**
### See [PhotonLib Java Examples](./README.md#swervedriveposeestsim)

0
photonlib-java-examples/apriltagExample/WPILib-License.md → photonlib-java-examples/swervedriveposeestsim/WPILib-License.md
View File

4
photonlib-java-examples/simposeest/build.gradle → photonlib-java-examples/swervedriveposeestsim/build.gradle
View File

@@ -19,7 +19,7 @@ deploy {
// or from command line. If not found an exception will be thrown. // or from command line. If not found an exception will be thrown.
// You can use getTeamOrDefault(team) instead of getTeamNumber if you // You can use getTeamOrDefault(team) instead of getTeamNumber if you
// want to store a team number in this file. // want to store a team number in this file.
team = project.frc.getTeamOrDefault(5940) team = project.frc.getTeamOrDefault(4512)
debug = project.frc.getDebugOrDefault(false) debug = project.frc.getDebugOrDefault(false)
artifacts { artifacts {
@@ -48,7 +48,7 @@ wpi.java.debugJni = false
def includeDesktopSupport = true def includeDesktopSupport = true
// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
// Also defines JUnit 4. // Also defines JUnit 5.
dependencies { dependencies {
implementation wpi.java.deps.wpilib() implementation wpi.java.deps.wpilib()
implementation wpi.java.vendor.java() implementation wpi.java.vendor.java()

0
photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.jar → photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.jar vendored
View File

0
photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties → photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties vendored
View File

0
photonlib-java-examples/apriltagExample/gradlew → photonlib-java-examples/swervedriveposeestsim/gradlew vendored
View File

0
photonlib-java-examples/apriltagExample/gradlew.bat → photonlib-java-examples/swervedriveposeestsim/gradlew.bat vendored
View File

0
photonlib-java-examples/apriltagExample/settings.gradle → photonlib-java-examples/swervedriveposeestsim/settings.gradle
View File

14
photonlib-java-examples/simposeest/simgui-ds.json → photonlib-java-examples/swervedriveposeestsim/simgui-ds.json
View File

@@ -1,9 +1,4 @@
{ {
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"keyboardJoysticks": [ "keyboardJoysticks": [
{ {
"axisConfig": [ "axisConfig": [
@@ -16,13 +11,16 @@
"incKey": 83 "incKey": 83
}, },
{ {
"decKey": 69,
"decayRate": 0.0, "decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582 "keyRate": 0.009999999776482582
},
{},
{
"decKey": 81,
"incKey": 69
} }
], ],
"axisCount": 3, "axisCount": 6,
"buttonCount": 4, "buttonCount": 4,
"buttonKeys": [ "buttonKeys": [
90, 90,

103
photonlib-java-examples/swervedriveposeestsim/simgui.json Normal file
View File

@@ -0,0 +1,103 @@
{
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
},
"windows": {
"/SmartDashboard/VisionSystemSim-main/Sim Field": {
"EstimatedRobot": {
"arrowWeight": 3.0,
"length": 0.800000011920929,
"selectable": false,
"weight": 3.0,
"width": 0.800000011920929
},
"EstimatedRobotModules": {
"arrows": false,
"image": "swerve_module.png",
"length": 0.30000001192092896,
"selectable": false,
"width": 0.30000001192092896
},
"Robot": {
"arrowColor": [
1.0,
1.0,
1.0,
255.0
],
"arrowWeight": 2.0,
"color": [
1.0,
1.0,
1.0,
255.0
],
"length": 0.800000011920929,
"selectable": false,
"weight": 2.0,
"width": 0.800000011920929
},
"VisionEstimation": {
"arrowColor": [
0.0,
0.6075949668884277,
1.0,
255.0
],
"arrowWeight": 2.0,
"color": [
0.0,
0.6075949668884277,
1.0,
255.0
],
"selectable": false,
"weight": 2.0
},
"apriltag": {
"image": "tag-green.png",
"length": 0.5,
"width": 0.4000000059604645
},
"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.013699531555176,
"visibleTargetPoses": {
"arrows": false,
"image": "tag-blue.png",
"length": 0.5,
"selectable": false,
"width": 0.4000000059604645
},
"width": 16.541749954223633,
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"transitory": {
"SmartDashboard": {
"VisionSystemSim-main": {
"open": true
},
"open": true
}
}
}
}

0
photonlib-java-examples/apriltagExample/src/main/deploy/example.txt → photonlib-java-examples/swervedriveposeestsim/src/main/deploy/example.txt
View File

151
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java Normal file
View File

@@ -0,0 +1,151 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import java.io.IOException;
public class Constants {
public static class Vision {
public static final String kCameraName = "YOUR CAMERA NAME";
// Cam mounted facing forward, half a meter forward of center, half a meter up from center.
public static final Transform3d kRobotToCam =
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0));
// The layout of the AprilTags on the field
public static final AprilTagFieldLayout kTagLayout;
static {
try {
kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
} catch (IOException e) {
e.printStackTrace();
throw new RuntimeException(e);
}
}
// The standard deviations of our vision estimated poses, which affect correction rate
// (Fake values. Experiment and determine estimation noise on an actual robot.)
public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
}
public static class Swerve {
// Physical properties
public static final double kTrackWidth = Units.inchesToMeters(18.5);
public static final double kTrackLength = Units.inchesToMeters(18.5);
public static final double kRobotWidth = Units.inchesToMeters(25 + 3.25 * 2);
public static final double kRobotLength = Units.inchesToMeters(25 + 3.25 * 2);
public static final double kMaxLinearSpeed = Units.feetToMeters(15.5);
public static final double kMaxAngularSpeed = Units.rotationsToRadians(2);
public static final double kWheelDiameter = Units.inchesToMeters(4);
public static final double kWheelCircumference = kWheelDiameter * Math.PI;
public static final double kDriveGearRatio = 6.75; // 6.75:1 SDS MK4 L2 ratio
public static final double kSteerGearRatio = 12.8; // 12.8:1
public static final double kDriveDistPerPulse = kWheelCircumference / 1024 / kDriveGearRatio;
public static final double kSteerRadPerPulse = 2 * Math.PI / 1024;
public enum ModuleConstants {
FL( // Front left
1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2),
FR( // Front Right
2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2),
BL( // Back Left
3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2),
BR( // Back Right
4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2);
public final int moduleNum;
public final int driveMotorID;
public final int driveEncoderA;
public final int driveEncoderB;
public final int steerMotorID;
public final int steerEncoderA;
public final int steerEncoderB;
public final double angleOffset;
public final Translation2d centerOffset;
private ModuleConstants(
int moduleNum,
int driveMotorID,
int driveEncoderA,
int driveEncoderB,
int steerMotorID,
int steerEncoderA,
int steerEncoderB,
double angleOffset,
double xOffset,
double yOffset) {
this.moduleNum = moduleNum;
this.driveMotorID = driveMotorID;
this.driveEncoderA = driveEncoderA;
this.driveEncoderB = driveEncoderB;
this.steerMotorID = steerMotorID;
this.steerEncoderA = steerEncoderA;
this.steerEncoderB = steerEncoderB;
this.angleOffset = angleOffset;
centerOffset = new Translation2d(xOffset, yOffset);
}
}
// Feedforward
// Linear drive feed forward
public static final SimpleMotorFeedforward kDriveFF =
new SimpleMotorFeedforward( // real
0.25, // Voltage to break static friction
2.5, // Volts per meter per second
0.3 // Volts per meter per second squared
);
// Steer feed forward
public static final SimpleMotorFeedforward kSteerFF =
new SimpleMotorFeedforward( // real
0.5, // Voltage to break static friction
0.25, // Volts per radian per second
0.01 // Volts per radian per second squared
);
// PID
public static final double kDriveKP = 1;
public static final double kDriveKI = 0;
public static final double kDriveKD = 0;
public static final double kSteerKP = 20;
public static final double kSteerKI = 0;
public static final double kSteerKD = 0.25;
}
}

10
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java → photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Main.java
View File

@@ -26,19 +26,9 @@ package frc.robot;
import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main { public final class Main {
private Main() {} private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) { public static void main(String... args) {
RobotBase.startRobot(Robot::new); RobotBase.startRobot(Robot::new);
} }

155
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java Normal file
View File

@@ -0,0 +1,155 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import frc.robot.subsystems.drivetrain.SwerveDrive;
import java.util.Random;
public class Robot extends TimedRobot {
private SwerveDrive drivetrain;
private Vision vision;
private XboxController controller;
// Limit max speed
private final double kDriveSpeed = 0.6;
// Rudimentary limiting of drivetrain acceleration
private SlewRateLimiter forwardLimiter = new SlewRateLimiter(1.0 / 0.6); // 1 / x seconds to 100%
private SlewRateLimiter strafeLimiter = new SlewRateLimiter(1.0 / 0.6);
private SlewRateLimiter turnLimiter = new SlewRateLimiter(1.0 / 0.33);
private Timer autoTimer = new Timer();
private Random rand = new Random(4512);
@Override
public void robotInit() {
drivetrain = new SwerveDrive();
vision = new Vision();
controller = new XboxController(0);
}
@Override
public void robotPeriodic() {
drivetrain.periodic();
// Correct pose estimate with vision measurements
var visionEst = vision.getEstimatedGlobalPose();
visionEst.ifPresent(
est -> {
var estPose = est.estimatedPose.toPose2d();
// Change our trust in the measurement based on the tags we can see
var estStdDevs = vision.getEstimationStdDevs(estPose);
drivetrain.addVisionMeasurement(
est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
});
// Apply a random offset to pose estimator to test vision correction
if (controller.getBButtonPressed()) {
var trf =
new Transform2d(
new Translation2d(rand.nextDouble() * 4 - 2, rand.nextDouble() * 4 - 2),
new Rotation2d(rand.nextDouble() * 2 * Math.PI));
drivetrain.resetPose(drivetrain.getPose().plus(trf), false);
}
// Log values to the dashboard
drivetrain.log();
}
@Override
public void disabledPeriodic() {
drivetrain.stop();
}
@Override
public void autonomousInit() {
autoTimer.restart();
var pose = new Pose2d(1, 1, new Rotation2d());
drivetrain.resetPose(pose, true);
vision.resetSimPose(pose);
}
@Override
public void autonomousPeriodic() {
// translate diagonally while spinning
if (autoTimer.get() < 10) {
drivetrain.drive(0.5, 0.5, 0.5, false);
} else {
autoTimer.stop();
drivetrain.stop();
}
}
@Override
public void teleopPeriodic() {
// We will use an "arcade drive" scheme to turn joystick values into target robot speeds
// We want to get joystick values where pushing forward/left is positive
double forward = -controller.getLeftY() * kDriveSpeed;
if (Math.abs(forward) < 0.1) forward = 0; // deadband small values
forward = forwardLimiter.calculate(forward); // limit acceleration
double strafe = -controller.getLeftX() * kDriveSpeed;
if (Math.abs(strafe) < 0.1) strafe = 0;
strafe = strafeLimiter.calculate(strafe);
double turn = -controller.getRightX() * kDriveSpeed;
if (Math.abs(turn) < 0.1) turn = 0;
turn = turnLimiter.calculate(turn);
// Convert from joystick values to real target speeds
forward *= Constants.Swerve.kMaxLinearSpeed;
strafe *= Constants.Swerve.kMaxLinearSpeed;
turn *= Constants.Swerve.kMaxLinearSpeed;
// Command drivetrain motors based on target speeds
drivetrain.drive(forward, strafe, turn, true);
}
@Override
public void simulationPeriodic() {
// Update drivetrain simulation
drivetrain.simulationPeriodic();
// Update camera simulation
vision.simulationPeriodic(drivetrain.getSimPose());
var debugField = vision.getSimDebugField();
debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose());
debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses());
// Calculate battery voltage sag due to current draw
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw()));
}
}

161
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java Normal file
View File

@@ -0,0 +1,161 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import static frc.robot.Constants.Vision.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.targeting.PhotonPipelineResult;
public class Vision {
private final PhotonCamera camera;
private final PhotonPoseEstimator photonEstimator;
private double lastEstTimestamp = 0;
// Simulation
private PhotonCameraSim cameraSim;
private VisionSystemSim visionSim;
public Vision() {
camera = new PhotonCamera(kCameraName);
photonEstimator =
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP, camera, kRobotToCam);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
// ----- Simulation
if (Robot.isSimulation()) {
// Create the vision system simulation which handles cameras and targets on the field.
visionSim = new VisionSystemSim("main");
// Add all the AprilTags inside the tag layout as visible targets to this simulated field.
visionSim.addAprilTags(kTagLayout);
// Create simulated camera properties. These can be set to mimic your actual camera.
var cameraProp = new SimCameraProperties();
cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(90));
cameraProp.setCalibError(0.35, 0.10);
cameraProp.setFPS(15);
cameraProp.setAvgLatencyMs(50);
cameraProp.setLatencyStdDevMs(15);
// Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible
// targets.
cameraSim = new PhotonCameraSim(camera, cameraProp);
// Add the simulated camera to view the targets on this simulated field.
visionSim.addCamera(cameraSim, kRobotToCam);
cameraSim.enableDrawWireframe(true);
}
}
public PhotonPipelineResult getLatestResult() {
return camera.getLatestResult();
}
/**
* The latest estimated robot pose on the field from vision data. This may be empty. This should
* only be called once per loop.
*
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
* used for estimation.
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
var visionEst = photonEstimator.update();
double latestTimestamp = camera.getLatestResult().getTimestampSeconds();
boolean newResult = Math.abs(latestTimestamp - lastEstTimestamp) > 1e-5;
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(
est ->
getSimDebugField()
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
if (newResult) getSimDebugField().getObject("VisionEstimation").setPoses();
});
}
if (newResult) lastEstTimestamp = latestTimestamp;
return visionEst;
}
/**
* The standard deviations of the estimated pose from {@link #getEstimatedGlobalPose()}, for use
* with {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}.
* This should only be used when there are targets visible.
*
* @param estimatedPose The estimated pose to guess standard deviations for.
*/
public Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose) {
var estStdDevs = kSingleTagStdDevs;
var targets = getLatestResult().getTargets();
int numTags = 0;
double avgDist = 0;
for (var tgt : targets) {
var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
if (tagPose.isEmpty()) continue;
numTags++;
avgDist +=
tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation());
}
if (numTags == 0) return estStdDevs;
avgDist /= numTags;
// Decrease std devs if multiple targets are visible
if (numTags > 1) estStdDevs = kMultiTagStdDevs;
// Increase std devs based on (average) distance
if (numTags == 1 && avgDist > 4)
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
return estStdDevs;
}
// ----- Simulation
public void simulationPeriodic(Pose2d robotSimPose) {
visionSim.update(robotSimPose);
}
/** Reset pose history of the robot in the vision system simulation. */
public void resetSimPose(Pose2d pose) {
if (Robot.isSimulation()) visionSim.resetRobotPose(pose);
}
/** A Field2d for visualizing our robot and objects on the field. */
public Field2d getSimDebugField() {
if (!Robot.isSimulation()) return null;
return visionSim.getDebugField();
}
}

334
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java Normal file
View File

@@ -0,0 +1,334 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.SPI.Port;
import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
public class SwerveDrive {
// Construct the swerve modules with their respective constants.
// The SwerveModule class will handle all the details of controlling the modules.
private final SwerveModule[] swerveMods = {
new SwerveModule(ModuleConstants.FL),
new SwerveModule(ModuleConstants.FR),
new SwerveModule(ModuleConstants.BL),
new SwerveModule(ModuleConstants.BR)
};
// The kinematics for translating between robot chassis speeds and module states.
private final SwerveDriveKinematics kinematics =
new SwerveDriveKinematics(
swerveMods[0].getModuleConstants().centerOffset,
swerveMods[1].getModuleConstants().centerOffset,
swerveMods[2].getModuleConstants().centerOffset,
swerveMods[3].getModuleConstants().centerOffset);
private final ADXRS450_Gyro gyro = new ADXRS450_Gyro(Port.kOnboardCS0);
// The robot pose estimator for tracking swerve odometry and applying vision corrections.
private final SwerveDrivePoseEstimator poseEstimator;
private ChassisSpeeds targetChassisSpeeds = new ChassisSpeeds();
// ----- Simulation
private final ADXRS450_GyroSim gyroSim;
private final SwerveDriveSim swerveDriveSim;
private double totalCurrentDraw = 0;
public SwerveDrive() {
// Define the standard deviations for the pose estimator, which determine how fast the pose
// estimate converges to the vision measurement. This should depend on the vision measurement
// noise
// and how many or how frequently vision measurements are applied to the pose estimator.
var stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
var visionStdDevs = VecBuilder.fill(1, 1, 1);
poseEstimator =
new SwerveDrivePoseEstimator(
kinematics,
getGyroYaw(),
getModulePositions(),
new Pose2d(),
stateStdDevs,
visionStdDevs);
// ----- Simulation
gyroSim = new ADXRS450_GyroSim(gyro);
swerveDriveSim =
new SwerveDriveSim(
kDriveFF,
DCMotor.getFalcon500(1),
kDriveGearRatio,
kWheelDiameter / 2.0,
kSteerFF,
DCMotor.getFalcon500(1),
kSteerGearRatio,
kinematics);
}
public void periodic() {
for (SwerveModule module : swerveMods) {
module.periodic();
}
// Update the odometry of the swerve drive using the wheel encoders and gyro.
poseEstimator.update(getGyroYaw(), getModulePositions());
}
/**
* Basic drive control. A target field-relative ChassisSpeeds (vx, vy, omega) is converted to
* specific swerve module states.
*
* @param vxMeters X velocity (forwards/backwards)
* @param vyMeters Y velocity (strafe left/right)
* @param omegaRadians Angular velocity (rotation CCW+)
* @param openLoop If swerve modules should use feedforward only and ignore velocity feedback
* control.
*/
public void drive(double vxMeters, double vyMeters, double omegaRadians, boolean openLoop) {
var targetChassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(vxMeters, vyMeters, omegaRadians, getHeading());
setChassisSpeeds(targetChassisSpeeds, openLoop, false);
}
/**
* Command the swerve drive to the desired chassis speeds by converting them to swerve module
* states and using {@link #setModuleStates(SwerveModuleState[], boolean)}.
*
* @param targetChassisSpeeds Target robot-relative chassis speeds (vx, vy, omega).
* @param openLoop If swerve modules should use feedforward only and ignore velocity feedback
* control.
* @param steerInPlace If modules should steer to the target angle when target velocity is 0.
*/
public void setChassisSpeeds(
ChassisSpeeds targetChassisSpeeds, boolean openLoop, boolean steerInPlace) {
setModuleStates(kinematics.toSwerveModuleStates(targetChassisSpeeds), openLoop, steerInPlace);
this.targetChassisSpeeds = targetChassisSpeeds;
}
/**
* Command the swerve modules to the desired states. Velocites exceeding the maximum speed will be
* desaturated (while preserving the ratios between modules).
*
* @param openLoop If swerve modules should use feedforward only and ignore velocity feedback
* control.
* @param steerInPlace If modules should steer to the target angle when target velocity is 0.
*/
public void setModuleStates(
SwerveModuleState[] desiredStates, boolean openLoop, boolean steerInPlace) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, kMaxLinearSpeed);
for (int i = 0; i < swerveMods.length; i++) {
swerveMods[i].setDesiredState(desiredStates[i], openLoop, steerInPlace);
}
}
/** Stop the swerve drive. */
public void stop() {
drive(0, 0, 0, true);
}
/** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. */
public void addVisionMeasurement(Pose2d visionMeasurement, double timestampSeconds) {
poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds);
}
/** See {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. */
public void addVisionMeasurement(
Pose2d visionMeasurement, double timestampSeconds, Matrix<N3, N1> stdDevs) {
poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs);
}
/**
* Reset the estimated pose of the swerve drive on the field.
*
* @param pose New robot pose.
* @param resetSimPose If the simulated robot pose should also be reset. This effectively
* teleports the robot and should only be used during the setup of the simulation world.
*/
public void resetPose(Pose2d pose, boolean resetSimPose) {
if (resetSimPose) {
swerveDriveSim.reset(pose, false);
// we shouldnt realistically be resetting pose after startup, but we will handle it anyway for
// testing
for (int i = 0; i < swerveMods.length; i++) {
swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0);
}
gyroSim.setAngle(-pose.getRotation().getDegrees());
gyroSim.setRate(0);
}
poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose);
}
/** Get the estimated pose of the swerve drive on the field. */
public Pose2d getPose() {
return poseEstimator.getEstimatedPosition();
}
/** The heading of the swerve drive's estimated pose on the field. */
public Rotation2d getHeading() {
return getPose().getRotation();
}
/** Raw gyro yaw (this may not match the field heading!). */
public Rotation2d getGyroYaw() {
return gyro.getRotation2d();
}
/** Get the chassis speeds of the robot (vx, vy, omega) from the swerve module states. */
public ChassisSpeeds getChassisSpeeds() {
return kinematics.toChassisSpeeds(getModuleStates());
}
/**
* Get the SwerveModuleState of each swerve module (speed, angle). The returned array order
* matches the kinematics module order.
*/
public SwerveModuleState[] getModuleStates() {
return new SwerveModuleState[] {
swerveMods[0].getState(),
swerveMods[1].getState(),
swerveMods[2].getState(),
swerveMods[3].getState()
};
}
/**
* Get the SwerveModulePosition of each swerve module (position, angle). The returned array order
* matches the kinematics module order.
*/
public SwerveModulePosition[] getModulePositions() {
return new SwerveModulePosition[] {
swerveMods[0].getPosition(),
swerveMods[1].getPosition(),
swerveMods[2].getPosition(),
swerveMods[3].getPosition()
};
}
/**
* Get the Pose2d of each swerve module based on kinematics and current robot pose. The returned
* array order matches the kinematics module order.
*/
public Pose2d[] getModulePoses() {
Pose2d[] modulePoses = new Pose2d[swerveMods.length];
for (int i = 0; i < swerveMods.length; i++) {
var module = swerveMods[i];
modulePoses[i] =
getPose()
.transformBy(
new Transform2d(
module.getModuleConstants().centerOffset, module.getAbsoluteHeading()));
}
return modulePoses;
}
/** Log various drivetrain values to the dashboard. */
public void log() {
String table = "Drive/";
Pose2d pose = getPose();
SmartDashboard.putNumber(table + "X", pose.getX());
SmartDashboard.putNumber(table + "Y", pose.getY());
SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees());
ChassisSpeeds chassisSpeeds = getChassisSpeeds();
SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber(
table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond));
SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber(
table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond));
for (SwerveModule module : swerveMods) {
module.log();
}
}
// ----- Simulation
public void simulationPeriodic() {
// Pass commanded motor voltages into swerve drive simulation
double[] driveInputs = new double[swerveMods.length];
double[] steerInputs = new double[swerveMods.length];
for (int i = 0; i < swerveMods.length; i++) {
driveInputs[i] = swerveMods[i].getDriveVoltage();
steerInputs[i] = swerveMods[i].getSteerVoltage();
}
swerveDriveSim.setDriveInputs(driveInputs);
swerveDriveSim.setSteerInputs(steerInputs);
// Simulate one timestep
swerveDriveSim.update(Robot.kDefaultPeriod);
// Update module and gyro values with simulated values
var driveStates = swerveDriveSim.getDriveStates();
var steerStates = swerveDriveSim.getSteerStates();
totalCurrentDraw = 0;
double[] driveCurrents = swerveDriveSim.getDriveCurrentDraw();
for (double current : driveCurrents) totalCurrentDraw += current;
double[] steerCurrents = swerveDriveSim.getSteerCurrentDraw();
for (double current : steerCurrents) totalCurrentDraw += current;
for (int i = 0; i < swerveMods.length; i++) {
double drivePos = driveStates.get(i).get(0, 0);
double driveRate = driveStates.get(i).get(1, 0);
double steerPos = steerStates.get(i).get(0, 0);
double steerRate = steerStates.get(i).get(1, 0);
swerveMods[i].simulationUpdate(
drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]);
}
gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec());
gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees());
}
/**
* The "actual" pose of the robot on the field used in simulation. This is different from the
* swerve drive's estimated pose.
*/
public Pose2d getSimPose() {
return swerveDriveSim.getPose();
}
public double getCurrentDraw() {
return totalCurrentDraw;
}
}

494
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java Normal file
View File

@@ -0,0 +1,494 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
/**
* This class attempts to simulate the dynamics of a swerve drive. In simulationPeriodic, users
* should first set inputs from motors with {@link #setDriveInputs(double...)} and {@link
* #setSteerInputs(double...)}, call {@link #update(double)} to update the simulation, and then set
* swerve module's encoder values and the drivetrain's gyro values with the results from this class.
*
* <p>In this class, distances are expressed with meters, angles with radians, and inputs with
* voltages.
*
* <p>Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on
* the Sim GUI's field.
*/
public class SwerveDriveSim {
private final LinearSystem<N2, N1, N2> drivePlant;
private final double driveKs;
private final DCMotor driveMotor;
private final double driveGearing;
private final double driveWheelRadius;
private final LinearSystem<N2, N1, N2> steerPlant;
private final double steerKs;
private final DCMotor steerMotor;
private final double steerGearing;
private final SwerveDriveKinematics kinematics;
private final int numModules;
private final double[] driveInputs;
private final List<Matrix<N2, N1>> driveStates;
private final double[] steerInputs;
private final List<Matrix<N2, N1>> steerStates;
private final Random rand = new Random();
// noiseless "actual" pose of the robot on the field
private Pose2d pose = new Pose2d();
private double omegaRadsPerSec = 0;
/**
* Creates a swerve drive simulation.
*
* @param driveFF The feedforward for the drive motors of this swerve drive. This should be in
* units of meters.
* @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction
* where one rotation of the drive wheel equals driveGearing rotations of the drive motor.
* @param driveWheelRadius The radius of the module's driving wheel.
* @param steerFF The feedforward for the steer motors of this swerve drive. This should be in
* units of radians.
* @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction
* where one rotation of the module heading/azimuth equals steerGearing rotations of the steer
* motor.
* @param kinematics The kinematics for this swerve drive. All swerve module information used in
* this class should match the order of the modules this kinematics object was constructed
* with.
*/
public SwerveDriveSim(
SimpleMotorFeedforward driveFF,
DCMotor driveMotor,
double driveGearing,
double driveWheelRadius,
SimpleMotorFeedforward steerFF,
DCMotor steerMotor,
double steerGearing,
SwerveDriveKinematics kinematics) {
this(
new LinearSystem<N2, N1, N2>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka),
VecBuilder.fill(0.0, 1.0 / driveFF.ka),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
driveFF.ks,
driveMotor,
driveGearing,
driveWheelRadius,
new LinearSystem<N2, N1, N2>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka),
VecBuilder.fill(0.0, 1.0 / steerFF.ka),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
steerFF.ks,
steerMotor,
steerGearing,
kinematics);
}
/**
* Creates a swerve drive simulation.
*
* @param drivePlant The {@link LinearSystem} representing a swerve module's drive system. The
* state should be in units of meters and input in volts.
* @param driveKs The static gain in volts of the drive system's feedforward, or the minimum
* voltage to cause motion. Set this to 0 to ignore static friction.
* @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction
* where one rotation of the drive wheel equals driveGearing rotations of the drive motor.
* @param driveWheelRadius The radius of the module's driving wheel.
* @param steerPlant The {@link LinearSystem} representing a swerve module's steer system. The
* state should be in units of radians and input in volts.
* @param steerKs The static gain in volts of the steer system's feedforward, or the minimum
* voltage to cause motion. Set this to 0 to ignore static friction.
* @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This
* should not have any gearing applied.
* @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction
* where one rotation of the module heading/azimuth equals steerGearing rotations of the steer
* motor.
* @param kinematics The kinematics for this swerve drive. All swerve module information used in
* this class should match the order of the modules this kinematics object was constructed
* with.
*/
public SwerveDriveSim(
LinearSystem<N2, N1, N2> drivePlant,
double driveKs,
DCMotor driveMotor,
double driveGearing,
double driveWheelRadius,
LinearSystem<N2, N1, N2> steerPlant,
double steerKs,
DCMotor steerMotor,
double steerGearing,
SwerveDriveKinematics kinematics) {
this.drivePlant = drivePlant;
this.driveKs = driveKs;
this.driveMotor = driveMotor;
this.driveGearing = driveGearing;
this.driveWheelRadius = driveWheelRadius;
this.steerPlant = steerPlant;
this.steerKs = steerKs;
this.steerMotor = steerMotor;
this.steerGearing = steerGearing;
this.kinematics = kinematics;
numModules = kinematics.toSwerveModuleStates(new ChassisSpeeds()).length;
driveInputs = new double[numModules];
driveStates = new ArrayList<>(numModules);
steerInputs = new double[numModules];
steerStates = new ArrayList<>(numModules);
for (int i = 0; i < numModules; i++) {
driveStates.add(VecBuilder.fill(0, 0));
steerStates.add(VecBuilder.fill(0, 0));
}
}
/**
* Sets the input voltages of the drive motors.
*
* @param inputs Input voltages. These should match the module order used in the kinematics.
*/
public void setDriveInputs(double... inputs) {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < driveInputs.length; i++) {
double input = inputs[i];
driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
}
}
/**
* Sets the input voltages of the steer motors.
*
* @param inputs Input voltages. These should match the module order used in the kinematics.
*/
public void setSteerInputs(double... inputs) {
final double battVoltage = RobotController.getBatteryVoltage();
for (int i = 0; i < steerInputs.length; i++) {
double input = inputs[i];
steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage);
}
}
/**
* Computes the new x given the old x and the control input. Includes the effect of static
* friction.
*
* @param discA The discretized system matrix.
* @param discB The discretized input matrix.
* @param x The position/velocity state of the drive/steer system.
* @param input The input voltage.
* @param ks The kS value of the feedforward model.
* @return The updated x, including the effect of static friction.
*/
protected static Matrix<N2, N1> calculateX(
Matrix<N2, N2> discA, Matrix<N2, N1> discB, Matrix<N2, N1> x, double input, double ks) {
var Ax = discA.times(x);
double nextStateVel = Ax.get(1, 0);
// input required to make next state vel == 0
double inputToStop = nextStateVel / -discB.get(1, 0);
// ks effect on system velocity
double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks);
// after ks system effect:
nextStateVel += discB.get(1, 0) * ksSystemEffect;
inputToStop = nextStateVel / -discB.get(1, 0);
double signToStop = Math.signum(inputToStop);
double inputSign = Math.signum(input);
double ksInputEffect = 0;
// system velocity was reduced to 0, resist any input
if (Math.abs(ksSystemEffect) < ks) {
double absInput = Math.abs(input);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
}
// non-zero system velocity, but input causes velocity sign change. Resist input after sign
// change
else if ((input * signToStop) > (inputToStop * signToStop)) {
double absInput = Math.abs(input - inputToStop);
ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput);
}
// calculate next x including static friction
var Bu = discB.times(VecBuilder.fill(input + ksSystemEffect + ksInputEffect));
return Ax.plus(Bu);
}
/**
* Update the drivetrain states with the given timestep.
*
* @param dtSeconds The timestep in seconds. This should be the robot loop period.
*/
public void update(double dtSeconds) {
var driveDiscAB = Discretization.discretizeAB(drivePlant.getA(), drivePlant.getB(), dtSeconds);
var steerDiscAB = Discretization.discretizeAB(steerPlant.getA(), steerPlant.getB(), dtSeconds);
var moduleDeltas = new SwerveModulePosition[numModules];
for (int i = 0; i < numModules; i++) {
double prevDriveStatePos = driveStates.get(i).get(0, 0);
driveStates.set(
i,
calculateX(
driveDiscAB.getFirst(),
driveDiscAB.getSecond(),
driveStates.get(i),
driveInputs[i],
driveKs));
double currDriveStatePos = driveStates.get(i).get(0, 0);
steerStates.set(
i,
calculateX(
steerDiscAB.getFirst(),
steerDiscAB.getSecond(),
steerStates.get(i),
steerInputs[i],
steerKs));
double currSteerStatePos = steerStates.get(i).get(0, 0);
moduleDeltas[i] =
new SwerveModulePosition(
currDriveStatePos - prevDriveStatePos, new Rotation2d(currSteerStatePos));
}
var twist = kinematics.toTwist2d(moduleDeltas);
pose = pose.exp(twist);
omegaRadsPerSec = twist.dtheta / dtSeconds;
}
/**
* Reset the simulated swerve drive state. This effectively teleports the robot and should only be
* used during the setup of the simulation world.
*
* @param pose The new pose of the simulated swerve drive.
* @param preserveMotion If true, the current module states will be preserved. Otherwise, they
* will be reset to zeros.
*/
public void reset(Pose2d pose, boolean preserveMotion) {
this.pose = pose;
if (!preserveMotion) {
for (int i = 0; i < numModules; i++) {
driveStates.set(i, VecBuilder.fill(0, 0));
steerStates.set(i, VecBuilder.fill(0, 0));
}
omegaRadsPerSec = 0;
}
}
/**
* Reset the simulated swerve drive state. This effectively teleports the robot and should only be
* used during the setup of the simulation world.
*
* @param pose The new pose of the simulated swerve drive.
* @param moduleDriveStates The new states of the modules' drive systems in [meters, meters/sec].
* These should match the module order used in the kinematics.
* @param moduleSteerStates The new states of the modules' steer systems in [radians,
* radians/sec]. These should match the module order used in the kinematics.
*/
public void reset(
Pose2d pose, List<Matrix<N2, N1>> moduleDriveStates, List<Matrix<N2, N1>> moduleSteerStates) {
if (moduleDriveStates.size() != driveStates.size()
|| moduleSteerStates.size() != steerStates.size())
throw new IllegalArgumentException("Provided module states do not match number of modules!");
this.pose = pose;
for (int i = 0; i < numModules; i++) {
driveStates.set(i, moduleDriveStates.get(i).copy());
steerStates.set(i, moduleSteerStates.get(i).copy());
}
omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond;
}
/**
* Get the pose of the simulated swerve drive. Note that this is the "actual" pose of the robot in
* the simulation world, without any noise. If you are simulating a pose estimator, this pose
* should only be used for visualization or camera simulation. This should be called after {@link
* #update(double)}.
*/
public Pose2d getPose() {
return pose;
}
/**
* Get the {@link SwerveModulePosition} of each module. The returned array order matches the
* kinematics module order. This should be called after {@link #update(double)}.
*/
public SwerveModulePosition[] getModulePositions() {
var positions = new SwerveModulePosition[numModules];
for (int i = 0; i < numModules; i++) {
positions[i] =
new SwerveModulePosition(
driveStates.get(i).get(0, 0), new Rotation2d(steerStates.get(i).get(0, 0)));
}
return positions;
}
/**
* Get the {@link SwerveModulePosition} of each module with rudimentary noise simulation. The
* returned array order matches the kinematics module order. This should be called after {@link
* #update(double)}.
*
* @param driveStdDev The standard deviation in meters of the drive wheel position.
* @param steerStdDev The standard deviation in radians of the steer angle.
*/
public SwerveModulePosition[] getNoisyModulePositions(double driveStdDev, double steerStdDev) {
var positions = new SwerveModulePosition[numModules];
for (int i = 0; i < numModules; i++) {
positions[i] =
new SwerveModulePosition(
driveStates.get(i).get(0, 0) + rand.nextGaussian() * driveStdDev,
new Rotation2d(steerStates.get(i).get(0, 0) + rand.nextGaussian() * steerStdDev));
}
return positions;
}
/**
* Get the {@link SwerveModuleState} of each module. The returned array order matches the
* kinematics module order. This should be called after {@link #update(double)}.
*/
public SwerveModuleState[] getModuleStates() {
var positions = new SwerveModuleState[numModules];
for (int i = 0; i < numModules; i++) {
positions[i] =
new SwerveModuleState(
driveStates.get(i).get(1, 0), new Rotation2d(steerStates.get(i).get(0, 0)));
}
return positions;
}
/**
* Get the state of each module's drive system in [meters, meters/sec]. The returned list order
* matches the kinematics module order. This should be called after {@link #update(double)}.
*/
public List<Matrix<N2, N1>> getDriveStates() {
List<Matrix<N2, N1>> states = new ArrayList<>();
for (int i = 0; i < driveStates.size(); i++) {
states.add(driveStates.get(i).copy());
}
return states;
}
/**
* Get the state of each module's steer system in [radians, radians/sec]. The returned list order
* matches the kinematics module order. This should be called after {@link #update(double)}.
*/
public List<Matrix<N2, N1>> getSteerStates() {
List<Matrix<N2, N1>> states = new ArrayList<>();
for (int i = 0; i < steerStates.size(); i++) {
states.add(steerStates.get(i).copy());
}
return states;
}
/**
* Get the angular velocity of the robot, which can be useful for gyro simulation. CCW positive.
* This should be called after {@link #update(double)}.
*/
public double getOmegaRadsPerSec() {
return omegaRadsPerSec;
}
/**
* Calculates the current drawn from the battery by the motor(s). Ignores regenerative current
* from back-emf.
*
* @param motor The motor(s) used.
* @param radiansPerSec The velocity of the motor in radians per second.
* @param inputVolts The voltage commanded by the motor controller (battery voltage * duty cycle).
* @param battVolts The voltage of the battery.
*/
protected static double getCurrentDraw(
DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) {
double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt;
// ignore regeneration
if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts);
else effVolts = MathUtil.clamp(effVolts, inputVolts, 0);
// calculate battery current
return (inputVolts / battVolts) * (effVolts / motor.rOhms);
}
/**
* Get the current draw in amps for each module's drive motor(s). This should be called after
* {@link #update(double)}. The returned array order matches the kinematics module order.
*/
public double[] getDriveCurrentDraw() {
double[] currents = new double[numModules];
for (int i = 0; i < numModules; i++) {
double radiansPerSec = driveStates.get(i).get(1, 0) * driveGearing / driveWheelRadius;
currents[i] =
getCurrentDraw(
driveMotor, radiansPerSec, driveInputs[i], RobotController.getBatteryVoltage());
}
return currents;
}
/**
* Get the current draw in amps for each module's steer motor(s). This should be called after
* {@link #update(double)}. The returned array order matches the kinematics module order.
*/
public double[] getSteerCurrentDraw() {
double[] currents = new double[numModules];
for (int i = 0; i < numModules; i++) {
double radiansPerSec = steerStates.get(i).get(1, 0) * steerGearing;
currents[i] =
getCurrentDraw(
steerMotor, radiansPerSec, steerInputs[i], RobotController.getBatteryVoltage());
}
return currents;
}
/**
* Get the total current draw in amps of all swerve motors. This should be called after {@link
* #update(double)}.
*/
public double getTotalCurrentDraw() {
double sum = 0;
for (double val : getDriveCurrentDraw()) sum += val;
for (double val : getSteerCurrentDraw()) sum += val;
return sum;
}
}

192
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java Normal file
View File

@@ -0,0 +1,192 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot.subsystems.drivetrain;
import static frc.robot.Constants.Swerve.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SwerveModule {
// --- Module Constants
private final ModuleConstants moduleConstants;
// --- Hardware
private final PWMSparkMax driveMotor;
private final Encoder driveEncoder;
private final PWMSparkMax steerMotor;
private final Encoder steerEncoder;
// --- Control
private SwerveModuleState desiredState = new SwerveModuleState();
private boolean openLoop = false;
// Simple PID feedback controllers run on the roborio
private PIDController drivePidController = new PIDController(kDriveKP, kDriveKI, kDriveKD);
// (A profiled steering PID controller may give better results by utilizing feedforward.)
private PIDController steerPidController = new PIDController(kSteerKP, kSteerKI, kSteerKD);
// --- Simulation
private final EncoderSim driveEncoderSim;
private double driveCurrentSim = 0;
private final EncoderSim steerEncoderSim;
private double steerCurrentSim = 0;
public SwerveModule(ModuleConstants moduleConstants) {
this.moduleConstants = moduleConstants;
driveMotor = new PWMSparkMax(moduleConstants.driveMotorID);
driveEncoder = new Encoder(moduleConstants.driveEncoderA, moduleConstants.driveEncoderB);
driveEncoder.setDistancePerPulse(kDriveDistPerPulse);
steerMotor = new PWMSparkMax(moduleConstants.steerMotorID);
steerEncoder = new Encoder(moduleConstants.steerEncoderA, moduleConstants.steerEncoderB);
steerEncoder.setDistancePerPulse(kSteerRadPerPulse);
steerPidController.enableContinuousInput(-Math.PI, Math.PI);
// --- Simulation
driveEncoderSim = new EncoderSim(driveEncoder);
steerEncoderSim = new EncoderSim(steerEncoder);
}
public void periodic() {
// Perform PID feedback control to steer the module to the target angle
double steerPid =
steerPidController.calculate(
getAbsoluteHeading().getRadians(), desiredState.angle.getRadians());
steerMotor.setVoltage(steerPid);
// Use feedforward model to translate target drive velocities into voltages
double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond);
double drivePid = 0;
if (!openLoop) {
// Perform PID feedback control to compensate for disturbances
drivePid =
drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond);
}
driveMotor.setVoltage(driveFF + drivePid);
}
/**
* Command this swerve module to the desired angle and velocity.
*
* @param steerInPlace If modules should steer to target angle when target velocity is 0
*/
public void setDesiredState(
SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) {
Rotation2d currentRotation = getAbsoluteHeading();
// Avoid turning more than 90 degrees by inverting speed on large angle changes
desiredState = SwerveModuleState.optimize(desiredState, currentRotation);
this.desiredState = desiredState;
}
/** Module heading reported by steering encoder. */
public Rotation2d getAbsoluteHeading() {
return Rotation2d.fromRadians(steerEncoder.getDistance());
}
/**
* {@link SwerveModuleState} describing absolute module rotation and velocity in meters per
* second.
*/
public SwerveModuleState getState() {
return new SwerveModuleState(driveEncoder.getRate(), getAbsoluteHeading());
}
/** {@link SwerveModulePosition} describing absolute module rotation and position in meters. */
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(driveEncoder.getDistance(), getAbsoluteHeading());
}
/** Voltage of the drive motor */
public double getDriveVoltage() {
return driveMotor.get() * RobotController.getBatteryVoltage();
}
/** Voltage of the steer motor */
public double getSteerVoltage() {
return steerMotor.get() * RobotController.getBatteryVoltage();
}
/**
* Constants about this module, like motor IDs, encoder angle offset, and translation from center.
*/
public ModuleConstants getModuleConstants() {
return moduleConstants;
}
public void log() {
var state = getState();
String table = "Module " + moduleConstants.moduleNum + "/";
SmartDashboard.putNumber(
table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians())));
SmartDashboard.putNumber(
table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint()));
SmartDashboard.putNumber(
table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond));
SmartDashboard.putNumber(
table + "Drive Velocity Target Feet",
Units.metersToFeet(desiredState.speedMetersPerSecond));
SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim);
SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim);
}
// ----- Simulation
public void simulationUpdate(
double driveEncoderDist,
double driveEncoderRate,
double driveCurrent,
double steerEncoderDist,
double steerEncoderRate,
double steerCurrent) {
driveEncoderSim.setDistance(driveEncoderDist);
driveEncoderSim.setRate(driveEncoderRate);
this.driveCurrentSim = driveCurrent;
steerEncoderSim.setDistance(steerEncoderDist);
steerEncoderSim.setRate(steerEncoderRate);
this.steerCurrentSim = steerCurrent;
}
public double getDriveCurrentSim() {
return driveCurrentSim;
}
public double getSteerCurrentSim() {
return steerCurrentSim;
}
}

BIN
photonlib-java-examples/swervedriveposeestsim/swerve_module.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.9 KiB

BIN
photonlib-java-examples/swervedriveposeestsim/tag-blue.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.6 KiB

BIN
photonlib-java-examples/swervedriveposeestsim/tag-green.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.6 KiB