From ce0d28da93b52cf6ef3459eaab994d21fe6b89a7 Mon Sep 17 00:00:00 2001 From: amquake Date: Thu, 5 Oct 2023 05:57:38 -0700 Subject: [PATCH] Update Java Simulation Examples (#913) Removes apriltagExample and simposeest, replacing them with swervedriveposeestsim --------- Co-authored-by: Matt --- .../simulation/VisionSystemSim.java | 26 +- photonlib-cpp-examples/README.md | 7 + photonlib-cpp-examples/build.gradle | 4 +- photonlib-cpp-examples/examples.gradle | 14 + photonlib-cpp-examples/examples.txt | 4 - photonlib-cpp-examples/settings.gradle | 4 +- photonlib-java-examples/README.md | 53 ++ photonlib-java-examples/aimandrange/README.md | 3 + photonlib-java-examples/aimattarget/README.md | 3 + .../.wpilib/wpilib_preferences.json | 6 - .../apriltagExample/build.gradle | 94 ---- .../apriltagExample/simgui-ds.json | 104 ---- .../apriltagExample/simgui.json | 63 --- .../src/main/java/frc/robot/Constants.java | 57 -- .../src/main/java/frc/robot/Drivetrain.java | 219 -------- .../java/frc/robot/PhotonCameraWrapper.java | 76 --- .../src/main/java/frc/robot/Robot.java | 101 ---- photonlib-java-examples/build.gradle | 4 +- photonlib-java-examples/examples.gradle | 14 + photonlib-java-examples/examples.txt | 6 - photonlib-java-examples/getinrange/README.md | 3 + photonlib-java-examples/settings.gradle | 7 +- .../simaimandrange/.gitignore | 1 + .../simaimandrange/README.md | 3 + .../simaimandrange/simgui-ds.json | 25 +- .../simaimandrange/simgui.json | 56 +- .../src/main/java/frc/robot/Robot.java | 20 +- .../java/frc/robot/sim/DrivetrainSim.java | 93 ++-- photonlib-java-examples/simposeest/.gitignore | 162 ------ .../simposeest/WPILib-License.md | 24 - .../simposeest/settings.gradle | 29 - .../simposeest/simgui.json | 57 -- .../simposeest/src/main/deploy/example.txt | 3 - .../main/java/frc/robot/AutoController.java | 110 ---- .../src/main/java/frc/robot/Constants.java | 110 ---- .../src/main/java/frc/robot/Drivetrain.java | 139 ----- .../frc/robot/DrivetrainPoseEstimator.java | 109 ---- .../main/java/frc/robot/DrivetrainSim.java | 172 ------ .../src/main/java/frc/robot/Main.java | 45 -- .../java/frc/robot/OperatorInterface.java | 57 -- .../main/java/frc/robot/PoseTelemetry.java | 61 --- .../src/main/java/frc/robot/Robot.java | 90 ---- .../.gitignore | 11 + .../.wpilib/wpilib_preferences.json | 2 +- .../swervedriveposeestsim/README.md | 3 + .../WPILib-License.md | 0 .../build.gradle | 4 +- .../gradle/wrapper/gradle-wrapper.jar | Bin .../gradle/wrapper/gradle-wrapper.properties | 0 .../gradlew | 0 .../gradlew.bat | 0 .../settings.gradle | 0 .../simgui-ds.json | 14 +- .../swervedriveposeestsim/simgui.json | 103 ++++ .../src/main/deploy/example.txt | 0 .../src/main/java/frc/robot/Constants.java | 151 ++++++ .../src/main/java/frc/robot/Main.java | 10 - .../src/main/java/frc/robot/Robot.java | 155 ++++++ .../src/main/java/frc/robot/Vision.java | 161 ++++++ .../subsystems/drivetrain/SwerveDrive.java | 334 ++++++++++++ .../subsystems/drivetrain/SwerveDriveSim.java | 494 ++++++++++++++++++ .../subsystems/drivetrain/SwerveModule.java | 192 +++++++ .../swervedriveposeestsim/swerve_module.png | Bin 0 -> 2954 bytes .../swervedriveposeestsim/tag-blue.png | Bin 0 -> 4711 bytes .../swervedriveposeestsim/tag-green.png | Bin 0 -> 4698 bytes 65 files changed, 1860 insertions(+), 2012 deletions(-) create mode 100644 photonlib-cpp-examples/README.md create mode 100644 photonlib-cpp-examples/examples.gradle delete mode 100644 photonlib-cpp-examples/examples.txt create mode 100644 photonlib-java-examples/README.md create mode 100644 photonlib-java-examples/aimandrange/README.md create mode 100644 photonlib-java-examples/aimattarget/README.md delete mode 100644 photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json delete mode 100644 photonlib-java-examples/apriltagExample/build.gradle delete mode 100644 photonlib-java-examples/apriltagExample/simgui-ds.json delete mode 100644 photonlib-java-examples/apriltagExample/simgui.json delete mode 100644 photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java delete mode 100644 photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java delete mode 100644 photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java delete mode 100644 photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java create mode 100644 photonlib-java-examples/examples.gradle delete mode 100644 photonlib-java-examples/examples.txt create mode 100644 photonlib-java-examples/getinrange/README.md create mode 100644 photonlib-java-examples/simaimandrange/README.md delete mode 100644 photonlib-java-examples/simposeest/.gitignore delete mode 100644 photonlib-java-examples/simposeest/WPILib-License.md delete mode 100644 photonlib-java-examples/simposeest/settings.gradle delete mode 100644 photonlib-java-examples/simposeest/simgui.json delete mode 100644 photonlib-java-examples/simposeest/src/main/deploy/example.txt delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/Main.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/OperatorInterface.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/PoseTelemetry.java delete mode 100644 photonlib-java-examples/simposeest/src/main/java/frc/robot/Robot.java rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/.gitignore (96%) rename photonlib-java-examples/{simposeest => swervedriveposeestsim}/.wpilib/wpilib_preferences.json (80%) create mode 100644 photonlib-java-examples/swervedriveposeestsim/README.md rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/WPILib-License.md (100%) rename photonlib-java-examples/{simposeest => swervedriveposeestsim}/build.gradle (97%) rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/gradle/wrapper/gradle-wrapper.jar (100%) rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/gradle/wrapper/gradle-wrapper.properties (100%) rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/gradlew (100%) rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/gradlew.bat (100%) rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/settings.gradle (100%) rename photonlib-java-examples/{simposeest => swervedriveposeestsim}/simgui-ds.json (91%) create mode 100644 photonlib-java-examples/swervedriveposeestsim/simgui.json rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/src/main/deploy/example.txt (100%) create mode 100644 photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java rename photonlib-java-examples/{apriltagExample => swervedriveposeestsim}/src/main/java/frc/robot/Main.java (77%) create mode 100644 photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java create mode 100644 photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java create mode 100644 photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java create mode 100644 photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java create mode 100644 photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java create mode 100644 photonlib-java-examples/swervedriveposeestsim/swerve_module.png create mode 100644 photonlib-java-examples/swervedriveposeestsim/tag-blue.png create mode 100644 photonlib-java-examples/swervedriveposeestsim/tag-green.png diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 4e65bdcbd4..ed0207096e 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -66,6 +66,8 @@ public class VisionSystemSim { 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 * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to @@ -337,7 +339,7 @@ public Field2d getDebugField() { * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically * 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) { update(new Pose3d(robotPoseMeters)); @@ -347,7 +349,7 @@ public void update(Pose2d robotPoseMeters) { * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically * 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) { var targetTypes = targetSets.entrySet(); @@ -370,13 +372,15 @@ public void update(Pose3d robotPoseMeters) { var allTargets = new ArrayList(); targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue())); - var visibleTargets = new ArrayList(); - var cameraPose2ds = new ArrayList(); + var visTgtPoses2d = new ArrayList(); + var cameraPoses2d = new ArrayList(); + boolean processed = false; // process each camera for (var camSim : camSimMap.values()) { // check if this camera is ready to process and get latency var optTimestamp = camSim.consumeNextEntryTime(); if (optTimestamp.isEmpty()) continue; + else processed = true; // when this result "was" read by NT long timestampNT = optTimestamp.get(); // this result's processing latency in milliseconds @@ -387,7 +391,7 @@ public void update(Pose3d robotPoseMeters) { // use camera pose from the image capture timestamp Pose3d lateRobotPose = getRobotPose(timestampCapture); Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get()); - cameraPose2ds.add(lateCameraPose.toPose2d()); + cameraPoses2d.add(lateCameraPose.toPose2d()); // process a PhotonPipelineResult with visible targets var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets); @@ -395,14 +399,12 @@ public void update(Pose3d robotPoseMeters) { camSim.submitProcessedFrame(camResult, timestampNT); // display debug results 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) { - dbgField - .getObject("visibleTargetPoses") - .setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList())); - } - if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds); + if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d); + if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d); } } diff --git a/photonlib-cpp-examples/README.md b/photonlib-cpp-examples/README.md new file mode 100644 index 0000000000..0a600f1a03 --- /dev/null +++ b/photonlib-cpp-examples/README.md @@ -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). + +--- diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 3605779a96..cb4661a3a9 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -27,9 +27,11 @@ spotless { } } +apply from: "examples.gradle" + // Task that depends on the build task for every example task buildAllExamples { task -> - new File('examples.txt').eachLine { line -> + exampleFolderNames.each { line -> task.dependsOn(line + ":build") } } diff --git a/photonlib-cpp-examples/examples.gradle b/photonlib-cpp-examples/examples.gradle new file mode 100644 index 0000000000..976440f33f --- /dev/null +++ b/photonlib-cpp-examples/examples.gradle @@ -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 } diff --git a/photonlib-cpp-examples/examples.txt b/photonlib-cpp-examples/examples.txt deleted file mode 100644 index 25f7648520..0000000000 --- a/photonlib-cpp-examples/examples.txt +++ /dev/null @@ -1,4 +0,0 @@ -aimandrange -getinrange -aimattarget -apriltagExample diff --git a/photonlib-cpp-examples/settings.gradle b/photonlib-cpp-examples/settings.gradle index 51a82a8d56..4c125fe51c 100644 --- a/photonlib-cpp-examples/settings.gradle +++ b/photonlib-cpp-examples/settings.gradle @@ -1 +1,3 @@ -new File('examples.txt').eachLine { line -> include line } +apply from: "examples.gradle" + +exampleFolderNames.each { line -> include line } diff --git a/photonlib-java-examples/README.md b/photonlib-java-examples/README.md new file mode 100644 index 0000000000..d02d6d3a7e --- /dev/null +++ b/photonlib-java-examples/README.md @@ -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. + + + +**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. + + + + + +**Keyboard controls:** +- Translate field-relative: WASD +- Rotate counter/clockwise: Q/E +- Offset pose estimate: X diff --git a/photonlib-java-examples/aimandrange/README.md b/photonlib-java-examples/aimandrange/README.md new file mode 100644 index 0000000000..718aed1b26 --- /dev/null +++ b/photonlib-java-examples/aimandrange/README.md @@ -0,0 +1,3 @@ +## **`aimandrange`** + +### See [PhotonLib Java Examples](./README.md#aimandrange) diff --git a/photonlib-java-examples/aimattarget/README.md b/photonlib-java-examples/aimattarget/README.md new file mode 100644 index 0000000000..f9cd1927a4 --- /dev/null +++ b/photonlib-java-examples/aimattarget/README.md @@ -0,0 +1,3 @@ +## **`aimattarget`** + +### See [PhotonLib Java Examples](./README.md#aimattarget) diff --git a/photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json b/photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json deleted file mode 100644 index 66deed1a92..0000000000 --- a/photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2023Beta", - "teamNumber": 1736 -} diff --git a/photonlib-java-examples/apriltagExample/build.gradle b/photonlib-java-examples/apriltagExample/build.gradle deleted file mode 100644 index fa1a30798c..0000000000 --- a/photonlib-java-examples/apriltagExample/build.gradle +++ /dev/null @@ -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' -} diff --git a/photonlib-java-examples/apriltagExample/simgui-ds.json b/photonlib-java-examples/apriltagExample/simgui-ds.json deleted file mode 100644 index 25e298be7f..0000000000 --- a/photonlib-java-examples/apriltagExample/simgui-ds.json +++ /dev/null @@ -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" - } - ] -} diff --git a/photonlib-java-examples/apriltagExample/simgui.json b/photonlib-java-examples/apriltagExample/simgui.json deleted file mode 100644 index 5c06de417f..0000000000 --- a/photonlib-java-examples/apriltagExample/simgui.json +++ /dev/null @@ -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 - } - } - } - } -} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java deleted file mode 100644 index b1a19249b4..0000000000 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -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"; - } -} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java deleted file mode 100644 index dba9ee3bca..0000000000 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java +++ /dev/null @@ -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 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 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()); - } -} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java deleted file mode 100644 index d46957886c..0000000000 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java +++ /dev/null @@ -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 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(); - } -} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java deleted file mode 100644 index 78f0f50ab9..0000000000 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -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(); - } -} diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 3605779a96..29a90b45f9 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -2,6 +2,8 @@ plugins { id "com.diffplug.spotless" version "6.1.2" } +apply from: "examples.gradle" + allprojects { repositories { mavenCentral() @@ -29,7 +31,7 @@ spotless { // Task that depends on the build task for every example task buildAllExamples { task -> - new File('examples.txt').eachLine { line -> + exampleFolderNames.each { line -> task.dependsOn(line + ":build") } } diff --git a/photonlib-java-examples/examples.gradle b/photonlib-java-examples/examples.gradle new file mode 100644 index 0000000000..976440f33f --- /dev/null +++ b/photonlib-java-examples/examples.gradle @@ -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 } diff --git a/photonlib-java-examples/examples.txt b/photonlib-java-examples/examples.txt deleted file mode 100644 index 94803d29fb..0000000000 --- a/photonlib-java-examples/examples.txt +++ /dev/null @@ -1,6 +0,0 @@ -aimandrange -aimattarget -getinrange -simaimandrange -simposeest -apriltagExample diff --git a/photonlib-java-examples/getinrange/README.md b/photonlib-java-examples/getinrange/README.md new file mode 100644 index 0000000000..2072891fca --- /dev/null +++ b/photonlib-java-examples/getinrange/README.md @@ -0,0 +1,3 @@ +## **`getinrange`** + +### See [PhotonLib Java Examples](./README.md#getinrange) diff --git a/photonlib-java-examples/settings.gradle b/photonlib-java-examples/settings.gradle index 63bcd788b7..4c125fe51c 100644 --- a/photonlib-java-examples/settings.gradle +++ b/photonlib-java-examples/settings.gradle @@ -1,6 +1,3 @@ -include 'photon-targeting' -include 'photon-core' -include 'photon-server' -include 'photon-lib' +apply from: "examples.gradle" -new File('examples.txt').eachLine { line -> include line } +exampleFolderNames.each { line -> include line } diff --git a/photonlib-java-examples/simaimandrange/.gitignore b/photonlib-java-examples/simaimandrange/.gitignore index 9535c83037..9912213c2b 100644 --- a/photonlib-java-examples/simaimandrange/.gitignore +++ b/photonlib-java-examples/simaimandrange/.gitignore @@ -160,3 +160,4 @@ bin/ # Simulation GUI and other tools window save file *-window.json +networktables.json diff --git a/photonlib-java-examples/simaimandrange/README.md b/photonlib-java-examples/simaimandrange/README.md new file mode 100644 index 0000000000..f06248a2e0 --- /dev/null +++ b/photonlib-java-examples/simaimandrange/README.md @@ -0,0 +1,3 @@ +## **`simaimandrange`** + +### See [PhotonLib Java Examples](./README.md#simaimandrange) diff --git a/photonlib-java-examples/simaimandrange/simgui-ds.json b/photonlib-java-examples/simaimandrange/simgui-ds.json index 77d5b5b89e..8dc94bc6a6 100644 --- a/photonlib-java-examples/simaimandrange/simgui-ds.json +++ b/photonlib-java-examples/simaimandrange/simgui-ds.json @@ -1,28 +1,25 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ + {}, { "decKey": 87, "incKey": 83 }, - {}, { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 + "decayRate": 0.10000000149011612, + "incKey": 81, + "keyRate": 0.10000000149011612 + }, + { + "decayRate": 0.10000000149011612, + "incKey": 69, + "keyRate": 0.10000000149011612 }, - {}, - {}, { - "decKey": 68, - "incKey": 65 + "decKey": 65, + "incKey": 68 } ], "axisCount": 6, diff --git a/photonlib-java-examples/simaimandrange/simgui.json b/photonlib-java-examples/simaimandrange/simgui.json index 53fcc87a54..2ebe473318 100644 --- a/photonlib-java-examples/simaimandrange/simgui.json +++ b/photonlib-java-examples/simaimandrange/simgui.json @@ -2,16 +2,47 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/photonvision Sim Field": "Field2d" + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" }, "windows": { - "/SmartDashboard/Field": { - "window": { - "visible": true - } - }, - "/SmartDashboard/photonvision Sim Field": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Robot": { + "arrowColor": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "arrowWeight": 3.0, + "color": [ + 1.0, + 1.0, + 1.0, + 255.0 + ], + "weight": 3.0 + }, + "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": { "visible": true } @@ -21,7 +52,7 @@ "NetworkTables": { "transitory": { "SmartDashboard": { - "Field": { + "VisionSystemSim-main/Sim Field": { "open": true }, "open": true @@ -33,12 +64,5 @@ } } } - }, - "Plot": { - "Plot <0>": { - "window": { - "visible": false - } - } } } diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java index 6dc8b6fd9e..53197dd216 100644 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java @@ -56,8 +56,8 @@ public class Robot extends TimedRobot { PhotonCamera camera = new PhotonCamera("photonvision"); // PID constants should be tuned per robot - final double LINEAR_P = 2.0; - final double LINEAR_D = 0.0; + final double LINEAR_P = 0.5; + final double LINEAR_D = 0.1; PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D); final double ANGULAR_P = 0.03; @@ -71,6 +71,12 @@ public class Robot extends TimedRobot { PWMVictorSPX rightMotor = new PWMVictorSPX(1); DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor); + @Override + public void robotInit() { + leftMotor.setInverted(false); + rightMotor.setInverted(true); + } + @Override public void teleopPeriodic() { double forwardSpeed; @@ -91,11 +97,11 @@ public void teleopPeriodic() { Units.degreesToRadians(result.getBestTarget().getPitch())); // Use this range as the measurement we give to the PID controller. - // -1.0 required to ensure positive PID controller effort _increases_ range + // (This forwardSpeed must be positive to go forward.) forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS); // 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); } else { // If we have no targets, stay still. @@ -104,8 +110,8 @@ public void teleopPeriodic() { } } else { // Manual Driver Mode - forwardSpeed = -xboxController.getRightY(); - rotationSpeed = xboxController.getLeftX(); + forwardSpeed = -xboxController.getLeftY() * 0.75; + rotationSpeed = -xboxController.getRightX() * 0.75; } // Use our forward/turn speeds to control the drivetrain @@ -119,7 +125,7 @@ public void teleopPeriodic() { @Override public void simulationInit() { - dtSim = new DrivetrainSim(); + dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera); } @Override diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java index 2686a202ba..0034ac8772 100644 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java @@ -26,6 +26,7 @@ import edu.wpi.first.math.geometry.Pose2d; 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.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -38,11 +39,14 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; 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 org.photonvision.simulation.SimVisionSystem; -import org.photonvision.simulation.SimVisionTarget; +import java.util.List; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; /** * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated @@ -53,53 +57,48 @@ */ public class DrivetrainSim { // Simulated Motor Controllers - PWMSim leftLeader = new PWMSim(0); - PWMSim rightLeader = new PWMSim(1); + PWMSim leftLeader; + PWMSim rightLeader; // Simulation Physics // Configure these to match your drivetrain's physical dimensions // and characterization results. + double trackwidthMeters = Units.feetToMeters(2.0); LinearSystem 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 = new DifferentialDrivetrainSim( drivetrainSystem, DCMotor.getCIM(2), 8, - Units.feetToMeters(2.0), + trackwidthMeters, Units.inchesToMeters(6.0 / 2.0), null); // Simulated Vision System. // Configure these to match your PhotonVision Camera, // 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 camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters + double minTargetArea = 0.1; // percentage (0 - 100) double maxLEDRange = 20; // meters int camResolutionWidth = 640; // pixels int camResolutionHeight = 480; // pixels - double minTargetArea = 10; // square pixels - - SimVisionSystem simVision = - new SimVisionSystem( - "photonvision", - camDiagFOV, - new Transform3d( - new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)), - maxLEDRange, - camResolutionWidth, - camResolutionHeight, - minTargetArea); + PhotonCameraSim cameraSim; + + VisionSystemSim visionSim = new VisionSystemSim("main"); // See // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf // page 208 - 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 - double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters + TargetModel targetModel = + new TargetModel( + List.of( + new Translation3d(0, Units.inchesToMeters(-9.819867), Units.inchesToMeters(-8.5)), + new Translation3d(0, Units.inchesToMeters(9.819867), Units.inchesToMeters(-8.5)), + new Translation3d(0, Units.inchesToMeters(19.625), Units.inchesToMeters(8.5)), + new Translation3d(0, Units.inchesToMeters(-19.625), Units.inchesToMeters(8.5)))); // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf // pages 4 and 5 double tgtXPos = Units.feetToMeters(54); @@ -108,13 +107,36 @@ public class DrivetrainSim { Pose3d farTargetPose = new Pose3d( new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS), - new Rotation3d(0.0, 0.0, 0.0)); + new Rotation3d(0.0, 0.0, Math.PI)); + + public DrivetrainSim(int leftMotorPort, int rightMotorPort, PhotonCamera camera) { + leftLeader = new PWMSim(leftMotorPort); + rightLeader = new PWMSim(rightMotorPort); - Field2d field = new Field2d(); + // Make the vision target visible to this simulated field. + var visionTarget = new VisionTargetSim(farTargetPose, targetModel); + visionSim.addVisionTargets(visionTarget); - public DrivetrainSim() { - simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1)); - SmartDashboard.putData("Field", field); + // 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 void update() { } drivetrainSimulator.setInputs( - leftMotorCmd * RobotController.getInputVoltage(), - -rightMotorCmd * RobotController.getInputVoltage()); + leftMotorCmd * RobotController.getBatteryVoltage(), + -rightMotorCmd * RobotController.getBatteryVoltage()); drivetrainSimulator.update(0.02); // Update PhotonVision based on our new robot position. - simVision.processFrame(drivetrainSimulator.getPose()); - - field.setRobotPose(drivetrainSimulator.getPose()); + visionSim.update(drivetrainSimulator.getPose()); } /** @@ -149,5 +169,6 @@ public void update() { */ public void resetPose(Pose2d pose) { drivetrainSimulator.setPose(pose); + visionSim.resetRobotPose(pose); } } diff --git a/photonlib-java-examples/simposeest/.gitignore b/photonlib-java-examples/simposeest/.gitignore deleted file mode 100644 index 9535c83037..0000000000 --- a/photonlib-java-examples/simposeest/.gitignore +++ /dev/null @@ -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 diff --git a/photonlib-java-examples/simposeest/WPILib-License.md b/photonlib-java-examples/simposeest/WPILib-License.md deleted file mode 100644 index 3d5a824cad..0000000000 --- a/photonlib-java-examples/simposeest/WPILib-License.md +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-java-examples/simposeest/settings.gradle b/photonlib-java-examples/simposeest/settings.gradle deleted file mode 100644 index bf7fd8a7d3..0000000000 --- a/photonlib-java-examples/simposeest/settings.gradle +++ /dev/null @@ -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 - } - } -} diff --git a/photonlib-java-examples/simposeest/simgui.json b/photonlib-java-examples/simposeest/simgui.json deleted file mode 100644 index a31aae4fae..0000000000 --- a/photonlib-java-examples/simposeest/simgui.json +++ /dev/null @@ -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 - } - } -} diff --git a/photonlib-java-examples/simposeest/src/main/deploy/example.txt b/photonlib-java-examples/simposeest/src/main/deploy/example.txt deleted file mode 100644 index d6ec5cf830..0000000000 --- a/photonlib-java-examples/simposeest/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java deleted file mode 100644 index bb0ee5a9b4..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java +++ /dev/null @@ -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; - } -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java deleted file mode 100644 index e5e8ca9caa..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -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); -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java deleted file mode 100644 index 996739c95f..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java +++ /dev/null @@ -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(); - } -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java deleted file mode 100644 index 1b28def5f6..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java +++ /dev/null @@ -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 stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05); - Matrix localMeasurementStdDevs = VecBuilder.fill(0.01, 0.01, Units.degreesToRadians(0.1)); - Matrix 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(); - } -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java deleted file mode 100644 index 7d1eee2d47..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java +++ /dev/null @@ -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. - * - *

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

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

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/OperatorInterface.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/OperatorInterface.java deleted file mode 100644 index 95429a7e30..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/OperatorInterface.java +++ /dev/null @@ -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(); - } -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/PoseTelemetry.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/PoseTelemetry.java deleted file mode 100644 index 4897669eb2..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/PoseTelemetry.java +++ /dev/null @@ -1,61 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -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; - } -} diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Robot.java deleted file mode 100644 index 4acbb81f2c..0000000000 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -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); - } -} diff --git a/photonlib-java-examples/apriltagExample/.gitignore b/photonlib-java-examples/swervedriveposeestsim/.gitignore similarity index 96% rename from photonlib-java-examples/apriltagExample/.gitignore rename to photonlib-java-examples/swervedriveposeestsim/.gitignore index 9535c83037..9f76c3a543 100644 --- a/photonlib-java-examples/apriltagExample/.gitignore +++ b/photonlib-java-examples/swervedriveposeestsim/.gitignore @@ -158,5 +158,16 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json +networktables.json diff --git a/photonlib-java-examples/simposeest/.wpilib/wpilib_preferences.json b/photonlib-java-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json similarity index 80% rename from photonlib-java-examples/simposeest/.wpilib/wpilib_preferences.json rename to photonlib-java-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json index c63569f160..832704069b 100644 --- a/photonlib-java-examples/simposeest/.wpilib/wpilib_preferences.json +++ b/photonlib-java-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2023", - "teamNumber": 6995 + "teamNumber": 4512 } diff --git a/photonlib-java-examples/swervedriveposeestsim/README.md b/photonlib-java-examples/swervedriveposeestsim/README.md new file mode 100644 index 0000000000..85e8d497db --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/README.md @@ -0,0 +1,3 @@ +## **`swervedriveposeestsim`** + +### See [PhotonLib Java Examples](./README.md#swervedriveposeestsim) diff --git a/photonlib-java-examples/apriltagExample/WPILib-License.md b/photonlib-java-examples/swervedriveposeestsim/WPILib-License.md similarity index 100% rename from photonlib-java-examples/apriltagExample/WPILib-License.md rename to photonlib-java-examples/swervedriveposeestsim/WPILib-License.md diff --git a/photonlib-java-examples/simposeest/build.gradle b/photonlib-java-examples/swervedriveposeestsim/build.gradle similarity index 97% rename from photonlib-java-examples/simposeest/build.gradle rename to photonlib-java-examples/swervedriveposeestsim/build.gradle index fa1a30798c..7610d2a2ac 100644 --- a/photonlib-java-examples/simposeest/build.gradle +++ b/photonlib-java-examples/swervedriveposeestsim/build.gradle @@ -19,7 +19,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(5940) + team = project.frc.getTeamOrDefault(4512) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -48,7 +48,7 @@ wpi.java.debugJni = false def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. +// Also defines JUnit 5. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.jar b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.jar similarity index 100% rename from photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.jar rename to photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.jar diff --git a/photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties similarity index 100% rename from photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties rename to photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties diff --git a/photonlib-java-examples/apriltagExample/gradlew b/photonlib-java-examples/swervedriveposeestsim/gradlew similarity index 100% rename from photonlib-java-examples/apriltagExample/gradlew rename to photonlib-java-examples/swervedriveposeestsim/gradlew diff --git a/photonlib-java-examples/apriltagExample/gradlew.bat b/photonlib-java-examples/swervedriveposeestsim/gradlew.bat similarity index 100% rename from photonlib-java-examples/apriltagExample/gradlew.bat rename to photonlib-java-examples/swervedriveposeestsim/gradlew.bat diff --git a/photonlib-java-examples/apriltagExample/settings.gradle b/photonlib-java-examples/swervedriveposeestsim/settings.gradle similarity index 100% rename from photonlib-java-examples/apriltagExample/settings.gradle rename to photonlib-java-examples/swervedriveposeestsim/settings.gradle diff --git a/photonlib-java-examples/simposeest/simgui-ds.json b/photonlib-java-examples/swervedriveposeestsim/simgui-ds.json similarity index 91% rename from photonlib-java-examples/simposeest/simgui-ds.json rename to photonlib-java-examples/swervedriveposeestsim/simgui-ds.json index 2bf389ee40..addd5860ce 100644 --- a/photonlib-java-examples/simposeest/simgui-ds.json +++ b/photonlib-java-examples/swervedriveposeestsim/simgui-ds.json @@ -1,9 +1,4 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -16,13 +11,16 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/photonlib-java-examples/swervedriveposeestsim/simgui.json b/photonlib-java-examples/swervedriveposeestsim/simgui.json new file mode 100644 index 0000000000..23b949fb97 --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/simgui.json @@ -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 + } + } + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/deploy/example.txt b/photonlib-java-examples/swervedriveposeestsim/src/main/deploy/example.txt similarity index 100% rename from photonlib-java-examples/apriltagExample/src/main/deploy/example.txt rename to photonlib-java-examples/swervedriveposeestsim/src/main/deploy/example.txt diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000000..708d87a5b0 --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Constants.java @@ -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 kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } + + public static class Swerve { + // Physical properties + public static final double kTrackWidth = Units.inchesToMeters(18.5); + public static final double kTrackLength = Units.inchesToMeters(18.5); + public static final double kRobotWidth = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kRobotLength = Units.inchesToMeters(25 + 3.25 * 2); + public static final double kMaxLinearSpeed = Units.feetToMeters(15.5); + public static final double kMaxAngularSpeed = Units.rotationsToRadians(2); + public static final double kWheelDiameter = Units.inchesToMeters(4); + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + public static final double kDriveGearRatio = 6.75; // 6.75:1 SDS MK4 L2 ratio + public static final double kSteerGearRatio = 12.8; // 12.8:1 + + public static final double kDriveDistPerPulse = kWheelCircumference / 1024 / kDriveGearRatio; + public static final double kSteerRadPerPulse = 2 * Math.PI / 1024; + + public enum ModuleConstants { + FL( // Front left + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2), + FR( // Front Right + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2), + BL( // Back Left + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2), + BR( // Back Right + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2); + + public final int moduleNum; + public final int driveMotorID; + public final int driveEncoderA; + public final int driveEncoderB; + public final int steerMotorID; + public final int steerEncoderA; + public final int steerEncoderB; + public final double angleOffset; + public final Translation2d centerOffset; + + private ModuleConstants( + int moduleNum, + int driveMotorID, + int driveEncoderA, + int driveEncoderB, + int steerMotorID, + int steerEncoderA, + int steerEncoderB, + double angleOffset, + double xOffset, + double yOffset) { + this.moduleNum = moduleNum; + this.driveMotorID = driveMotorID; + this.driveEncoderA = driveEncoderA; + this.driveEncoderB = driveEncoderB; + this.steerMotorID = steerMotorID; + this.steerEncoderA = steerEncoderA; + this.steerEncoderB = steerEncoderB; + this.angleOffset = angleOffset; + centerOffset = new Translation2d(xOffset, yOffset); + } + } + + // Feedforward + // Linear drive feed forward + public static final SimpleMotorFeedforward kDriveFF = + new SimpleMotorFeedforward( // real + 0.25, // Voltage to break static friction + 2.5, // Volts per meter per second + 0.3 // Volts per meter per second squared + ); + // Steer feed forward + public static final SimpleMotorFeedforward kSteerFF = + new SimpleMotorFeedforward( // real + 0.5, // Voltage to break static friction + 0.25, // Volts per radian per second + 0.01 // Volts per radian per second squared + ); + + // PID + public static final double kDriveKP = 1; + public static final double kDriveKI = 0; + public static final double kDriveKD = 0; + + public static final double kSteerKP = 20; + public static final double kSteerKI = 0; + public static final double kSteerKD = 0.25; + } +} diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Main.java similarity index 77% rename from photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java rename to photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Main.java index 077f979f97..f227c28da0 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Main.java +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Main.java @@ -26,19 +26,9 @@ import edu.wpi.first.wpilibj.RobotBase; -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ public final class Main { private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000000..d32d4d477c --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Robot.java @@ -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())); + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java new file mode 100644 index 0000000000..1d9b2ab9ed --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java @@ -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 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 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(); + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java new file mode 100644 index 0000000000..21911c997d --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -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 stdDevs) { + poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); + } + + /** + * Reset the estimated pose of the swerve drive on the field. + * + * @param pose New robot pose. + * @param resetSimPose If the simulated robot pose should also be reset. This effectively + * teleports the robot and should only be used during the setup of the simulation world. + */ + public void resetPose(Pose2d pose, boolean resetSimPose) { + if (resetSimPose) { + swerveDriveSim.reset(pose, false); + // we shouldnt realistically be resetting pose after startup, but we will handle it anyway for + // testing + for (int i = 0; i < swerveMods.length; i++) { + swerveMods[i].simulationUpdate(0, 0, 0, 0, 0, 0); + } + gyroSim.setAngle(-pose.getRotation().getDegrees()); + gyroSim.setRate(0); + } + + poseEstimator.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + /** Get the estimated pose of the swerve drive on the field. */ + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** The heading of the swerve drive's estimated pose on the field. */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** Raw gyro yaw (this may not match the field heading!). */ + public Rotation2d getGyroYaw() { + return gyro.getRotation2d(); + } + + /** Get the chassis speeds of the robot (vx, vy, omega) from the swerve module states. */ + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + + /** + * Get the SwerveModuleState of each swerve module (speed, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + swerveMods[0].getState(), + swerveMods[1].getState(), + swerveMods[2].getState(), + swerveMods[3].getState() + }; + } + + /** + * Get the SwerveModulePosition of each swerve module (position, angle). The returned array order + * matches the kinematics module order. + */ + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + swerveMods[0].getPosition(), + swerveMods[1].getPosition(), + swerveMods[2].getPosition(), + swerveMods[3].getPosition() + }; + } + + /** + * Get the Pose2d of each swerve module based on kinematics and current robot pose. The returned + * array order matches the kinematics module order. + */ + public Pose2d[] getModulePoses() { + Pose2d[] modulePoses = new Pose2d[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + var module = swerveMods[i]; + modulePoses[i] = + getPose() + .transformBy( + new Transform2d( + module.getModuleConstants().centerOffset, module.getAbsoluteHeading())); + } + return modulePoses; + } + + /** Log various drivetrain values to the dashboard. */ + public void log() { + String table = "Drive/"; + Pose2d pose = getPose(); + SmartDashboard.putNumber(table + "X", pose.getX()); + SmartDashboard.putNumber(table + "Y", pose.getY()); + SmartDashboard.putNumber(table + "Heading", pose.getRotation().getDegrees()); + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + SmartDashboard.putNumber(table + "VX", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "VY", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Omega Degrees", Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond)); + SmartDashboard.putNumber(table + "Target VX", targetChassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber(table + "Target VY", targetChassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber( + table + "Target Omega Degrees", Math.toDegrees(targetChassisSpeeds.omegaRadiansPerSecond)); + + for (SwerveModule module : swerveMods) { + module.log(); + } + } + + // ----- Simulation + + public void simulationPeriodic() { + // Pass commanded motor voltages into swerve drive simulation + double[] driveInputs = new double[swerveMods.length]; + double[] steerInputs = new double[swerveMods.length]; + for (int i = 0; i < swerveMods.length; i++) { + driveInputs[i] = swerveMods[i].getDriveVoltage(); + steerInputs[i] = swerveMods[i].getSteerVoltage(); + } + swerveDriveSim.setDriveInputs(driveInputs); + swerveDriveSim.setSteerInputs(steerInputs); + + // Simulate one timestep + swerveDriveSim.update(Robot.kDefaultPeriod); + + // Update module and gyro values with simulated values + var driveStates = swerveDriveSim.getDriveStates(); + var steerStates = swerveDriveSim.getSteerStates(); + totalCurrentDraw = 0; + double[] driveCurrents = swerveDriveSim.getDriveCurrentDraw(); + for (double current : driveCurrents) totalCurrentDraw += current; + double[] steerCurrents = swerveDriveSim.getSteerCurrentDraw(); + for (double current : steerCurrents) totalCurrentDraw += current; + for (int i = 0; i < swerveMods.length; i++) { + double drivePos = driveStates.get(i).get(0, 0); + double driveRate = driveStates.get(i).get(1, 0); + double steerPos = steerStates.get(i).get(0, 0); + double steerRate = steerStates.get(i).get(1, 0); + swerveMods[i].simulationUpdate( + drivePos, driveRate, driveCurrents[i], steerPos, steerRate, steerCurrents[i]); + } + + gyroSim.setRate(-swerveDriveSim.getOmegaRadsPerSec()); + gyroSim.setAngle(-swerveDriveSim.getPose().getRotation().getDegrees()); + } + + /** + * The "actual" pose of the robot on the field used in simulation. This is different from the + * swerve drive's estimated pose. + */ + public Pose2d getSimPose() { + return swerveDriveSim.getPose(); + } + + public double getCurrentDraw() { + return totalCurrentDraw; + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java new file mode 100644 index 0000000000..aff69fc703 --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java @@ -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. + * + *

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

Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize their robot on + * the Sim GUI's field. + */ +public class SwerveDriveSim { + private final LinearSystem drivePlant; + private final double driveKs; + private final DCMotor driveMotor; + private final double driveGearing; + private final double driveWheelRadius; + private final LinearSystem steerPlant; + private final double steerKs; + private final DCMotor steerMotor; + private final double steerGearing; + + private final SwerveDriveKinematics kinematics; + private final int numModules; + + private final double[] driveInputs; + private final List> driveStates; + private final double[] steerInputs; + private final List> steerStates; + + private final Random rand = new Random(); + + // noiseless "actual" pose of the robot on the field + private Pose2d pose = new Pose2d(); + private double omegaRadsPerSec = 0; + + /** + * Creates a swerve drive simulation. + * + * @param driveFF The feedforward for the drive motors of this swerve drive. This should be in + * units of meters. + * @param driveMotor The DCMotor model for the drive motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param driveGearing The gear ratio of the drive system. Positive values indicate a reduction + * where one rotation of the drive wheel equals driveGearing rotations of the drive motor. + * @param driveWheelRadius The radius of the module's driving wheel. + * @param steerFF The feedforward for the steer motors of this swerve drive. This should be in + * units of radians. + * @param steerMotor The DCMotor model for the steer motor(s) of this swerve drive's modules. This + * should not have any gearing applied. + * @param steerGearing The gear ratio of the steer system. Positive values indicate a reduction + * where one rotation of the module heading/azimuth equals steerGearing rotations of the steer + * motor. + * @param kinematics The kinematics for this swerve drive. All swerve module information used in + * this class should match the order of the modules this kinematics object was constructed + * with. + */ + public SwerveDriveSim( + SimpleMotorFeedforward driveFF, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + SimpleMotorFeedforward steerFF, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this( + new LinearSystem( + 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( + 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 drivePlant, + double driveKs, + DCMotor driveMotor, + double driveGearing, + double driveWheelRadius, + LinearSystem steerPlant, + double steerKs, + DCMotor steerMotor, + double steerGearing, + SwerveDriveKinematics kinematics) { + this.drivePlant = drivePlant; + this.driveKs = driveKs; + this.driveMotor = driveMotor; + this.driveGearing = driveGearing; + this.driveWheelRadius = driveWheelRadius; + this.steerPlant = steerPlant; + this.steerKs = steerKs; + this.steerMotor = steerMotor; + this.steerGearing = steerGearing; + + this.kinematics = kinematics; + numModules = kinematics.toSwerveModuleStates(new ChassisSpeeds()).length; + driveInputs = new double[numModules]; + driveStates = new ArrayList<>(numModules); + steerInputs = new double[numModules]; + steerStates = new ArrayList<>(numModules); + for (int i = 0; i < numModules; i++) { + driveStates.add(VecBuilder.fill(0, 0)); + steerStates.add(VecBuilder.fill(0, 0)); + } + } + + /** + * Sets the input voltages of the drive motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setDriveInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < driveInputs.length; i++) { + double input = inputs[i]; + driveInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Sets the input voltages of the steer motors. + * + * @param inputs Input voltages. These should match the module order used in the kinematics. + */ + public void setSteerInputs(double... inputs) { + final double battVoltage = RobotController.getBatteryVoltage(); + for (int i = 0; i < steerInputs.length; i++) { + double input = inputs[i]; + steerInputs[i] = MathUtil.clamp(input, -battVoltage, battVoltage); + } + } + + /** + * Computes the new x given the old x and the control input. Includes the effect of static + * friction. + * + * @param discA The discretized system matrix. + * @param discB The discretized input matrix. + * @param x The position/velocity state of the drive/steer system. + * @param input The input voltage. + * @param ks The kS value of the feedforward model. + * @return The updated x, including the effect of static friction. + */ + protected static Matrix calculateX( + Matrix discA, Matrix discB, Matrix x, double input, double ks) { + var Ax = discA.times(x); + double nextStateVel = Ax.get(1, 0); + // input required to make next state vel == 0 + double inputToStop = nextStateVel / -discB.get(1, 0); + // ks effect on system velocity + double ksSystemEffect = MathUtil.clamp(inputToStop, -ks, ks); + + // after ks system effect: + nextStateVel += discB.get(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB.get(1, 0); + double signToStop = Math.signum(inputToStop); + double inputSign = Math.signum(input); + double ksInputEffect = 0; + + // system velocity was reduced to 0, resist any input + if (Math.abs(ksSystemEffect) < ks) { + double absInput = Math.abs(input); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + // non-zero system velocity, but input causes velocity sign change. Resist input after sign + // change + else if ((input * signToStop) > (inputToStop * signToStop)) { + double absInput = Math.abs(input - inputToStop); + ksInputEffect = -MathUtil.clamp(ks * inputSign, -absInput, absInput); + } + + // calculate next x including static friction + var Bu = discB.times(VecBuilder.fill(input + ksSystemEffect + ksInputEffect)); + return Ax.plus(Bu); + } + + /** + * Update the drivetrain states with the given timestep. + * + * @param dtSeconds The timestep in seconds. This should be the robot loop period. + */ + public void update(double dtSeconds) { + var driveDiscAB = Discretization.discretizeAB(drivePlant.getA(), drivePlant.getB(), dtSeconds); + var steerDiscAB = Discretization.discretizeAB(steerPlant.getA(), steerPlant.getB(), dtSeconds); + + var moduleDeltas = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates.get(i).get(0, 0); + driveStates.set( + i, + calculateX( + driveDiscAB.getFirst(), + driveDiscAB.getSecond(), + driveStates.get(i), + driveInputs[i], + driveKs)); + double currDriveStatePos = driveStates.get(i).get(0, 0); + steerStates.set( + i, + calculateX( + steerDiscAB.getFirst(), + steerDiscAB.getSecond(), + steerStates.get(i), + steerInputs[i], + steerKs)); + double currSteerStatePos = steerStates.get(i).get(0, 0); + moduleDeltas[i] = + new SwerveModulePosition( + currDriveStatePos - prevDriveStatePos, new Rotation2d(currSteerStatePos)); + } + + var twist = kinematics.toTwist2d(moduleDeltas); + pose = pose.exp(twist); + omegaRadsPerSec = twist.dtheta / dtSeconds; + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param preserveMotion If true, the current module states will be preserved. Otherwise, they + * will be reset to zeros. + */ + public void reset(Pose2d pose, boolean preserveMotion) { + this.pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates.set(i, VecBuilder.fill(0, 0)); + steerStates.set(i, VecBuilder.fill(0, 0)); + } + omegaRadsPerSec = 0; + } + } + + /** + * Reset the simulated swerve drive state. This effectively teleports the robot and should only be + * used during the setup of the simulation world. + * + * @param pose The new pose of the simulated swerve drive. + * @param moduleDriveStates The new states of the modules' drive systems in [meters, meters/sec]. + * These should match the module order used in the kinematics. + * @param moduleSteerStates The new states of the modules' steer systems in [radians, + * radians/sec]. These should match the module order used in the kinematics. + */ + public void reset( + Pose2d pose, List> moduleDriveStates, List> moduleSteerStates) { + if (moduleDriveStates.size() != driveStates.size() + || moduleSteerStates.size() != steerStates.size()) + throw new IllegalArgumentException("Provided module states do not match number of modules!"); + this.pose = pose; + for (int i = 0; i < numModules; i++) { + driveStates.set(i, moduleDriveStates.get(i).copy()); + steerStates.set(i, moduleSteerStates.get(i).copy()); + } + omegaRadsPerSec = kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond; + } + + /** + * Get the pose of the simulated swerve drive. Note that this is the "actual" pose of the robot in + * the simulation world, without any noise. If you are simulating a pose estimator, this pose + * should only be used for visualization or camera simulation. This should be called after {@link + * #update(double)}. + */ + public Pose2d getPose() { + return pose; + } + + /** + * Get the {@link SwerveModulePosition} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModulePosition[] getModulePositions() { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the {@link SwerveModulePosition} of each module with rudimentary noise simulation. The + * returned array order matches the kinematics module order. This should be called after {@link + * #update(double)}. + * + * @param driveStdDev The standard deviation in meters of the drive wheel position. + * @param steerStdDev The standard deviation in radians of the steer angle. + */ + public SwerveModulePosition[] getNoisyModulePositions(double driveStdDev, double steerStdDev) { + var positions = new SwerveModulePosition[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModulePosition( + driveStates.get(i).get(0, 0) + rand.nextGaussian() * driveStdDev, + new Rotation2d(steerStates.get(i).get(0, 0) + rand.nextGaussian() * steerStdDev)); + } + return positions; + } + + /** + * Get the {@link SwerveModuleState} of each module. The returned array order matches the + * kinematics module order. This should be called after {@link #update(double)}. + */ + public SwerveModuleState[] getModuleStates() { + var positions = new SwerveModuleState[numModules]; + for (int i = 0; i < numModules; i++) { + positions[i] = + new SwerveModuleState( + driveStates.get(i).get(1, 0), new Rotation2d(steerStates.get(i).get(0, 0))); + } + return positions; + } + + /** + * Get the state of each module's drive system in [meters, meters/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getDriveStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < driveStates.size(); i++) { + states.add(driveStates.get(i).copy()); + } + return states; + } + + /** + * Get the state of each module's steer system in [radians, radians/sec]. The returned list order + * matches the kinematics module order. This should be called after {@link #update(double)}. + */ + public List> getSteerStates() { + List> states = new ArrayList<>(); + for (int i = 0; i < steerStates.size(); i++) { + states.add(steerStates.get(i).copy()); + } + return states; + } + + /** + * Get the angular velocity of the robot, which can be useful for gyro simulation. CCW positive. + * This should be called after {@link #update(double)}. + */ + public double getOmegaRadsPerSec() { + return omegaRadsPerSec; + } + + /** + * Calculates the current drawn from the battery by the motor(s). Ignores regenerative current + * from back-emf. + * + * @param motor The motor(s) used. + * @param radiansPerSec The velocity of the motor in radians per second. + * @param inputVolts The voltage commanded by the motor controller (battery voltage * duty cycle). + * @param battVolts The voltage of the battery. + */ + protected static double getCurrentDraw( + DCMotor motor, double radiansPerSec, double inputVolts, double battVolts) { + double effVolts = inputVolts - radiansPerSec / motor.KvRadPerSecPerVolt; + // ignore regeneration + if (inputVolts >= 0) effVolts = MathUtil.clamp(effVolts, 0, inputVolts); + else effVolts = MathUtil.clamp(effVolts, inputVolts, 0); + // calculate battery current + return (inputVolts / battVolts) * (effVolts / motor.rOhms); + } + + /** + * Get the current draw in amps for each module's drive motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getDriveCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = driveStates.get(i).get(1, 0) * driveGearing / driveWheelRadius; + currents[i] = + getCurrentDraw( + driveMotor, radiansPerSec, driveInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the current draw in amps for each module's steer motor(s). This should be called after + * {@link #update(double)}. The returned array order matches the kinematics module order. + */ + public double[] getSteerCurrentDraw() { + double[] currents = new double[numModules]; + for (int i = 0; i < numModules; i++) { + double radiansPerSec = steerStates.get(i).get(1, 0) * steerGearing; + currents[i] = + getCurrentDraw( + steerMotor, radiansPerSec, steerInputs[i], RobotController.getBatteryVoltage()); + } + return currents; + } + + /** + * Get the total current draw in amps of all swerve motors. This should be called after {@link + * #update(double)}. + */ + public double getTotalCurrentDraw() { + double sum = 0; + for (double val : getDriveCurrentDraw()) sum += val; + for (double val : getSteerCurrentDraw()) sum += val; + return sum; + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java new file mode 100644 index 0000000000..4615d29217 --- /dev/null +++ b/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java @@ -0,0 +1,192 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems.drivetrain; + +import static frc.robot.Constants.Swerve.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + // --- Module Constants + private final ModuleConstants moduleConstants; + + // --- Hardware + private final PWMSparkMax driveMotor; + private final Encoder driveEncoder; + private final PWMSparkMax steerMotor; + private final Encoder steerEncoder; + + // --- Control + private SwerveModuleState desiredState = new SwerveModuleState(); + private boolean openLoop = false; + + // Simple PID feedback controllers run on the roborio + private PIDController drivePidController = new PIDController(kDriveKP, kDriveKI, kDriveKD); + // (A profiled steering PID controller may give better results by utilizing feedforward.) + private PIDController steerPidController = new PIDController(kSteerKP, kSteerKI, kSteerKD); + + // --- Simulation + private final EncoderSim driveEncoderSim; + private double driveCurrentSim = 0; + private final EncoderSim steerEncoderSim; + private double steerCurrentSim = 0; + + public SwerveModule(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + driveMotor = new PWMSparkMax(moduleConstants.driveMotorID); + driveEncoder = new Encoder(moduleConstants.driveEncoderA, moduleConstants.driveEncoderB); + driveEncoder.setDistancePerPulse(kDriveDistPerPulse); + steerMotor = new PWMSparkMax(moduleConstants.steerMotorID); + steerEncoder = new Encoder(moduleConstants.steerEncoderA, moduleConstants.steerEncoderB); + steerEncoder.setDistancePerPulse(kSteerRadPerPulse); + + steerPidController.enableContinuousInput(-Math.PI, Math.PI); + + // --- Simulation + driveEncoderSim = new EncoderSim(driveEncoder); + steerEncoderSim = new EncoderSim(steerEncoder); + } + + public void periodic() { + // Perform PID feedback control to steer the module to the target angle + double steerPid = + steerPidController.calculate( + getAbsoluteHeading().getRadians(), desiredState.angle.getRadians()); + steerMotor.setVoltage(steerPid); + + // Use feedforward model to translate target drive velocities into voltages + double driveFF = kDriveFF.calculate(desiredState.speedMetersPerSecond); + double drivePid = 0; + if (!openLoop) { + // Perform PID feedback control to compensate for disturbances + drivePid = + drivePidController.calculate(driveEncoder.getRate(), desiredState.speedMetersPerSecond); + } + + driveMotor.setVoltage(driveFF + drivePid); + } + + /** + * Command this swerve module to the desired angle and velocity. + * + * @param steerInPlace If modules should steer to target angle when target velocity is 0 + */ + public void setDesiredState( + SwerveModuleState desiredState, boolean openLoop, boolean steerInPlace) { + Rotation2d currentRotation = getAbsoluteHeading(); + // Avoid turning more than 90 degrees by inverting speed on large angle changes + desiredState = SwerveModuleState.optimize(desiredState, currentRotation); + + this.desiredState = desiredState; + } + + /** Module heading reported by steering encoder. */ + public Rotation2d getAbsoluteHeading() { + return Rotation2d.fromRadians(steerEncoder.getDistance()); + } + + /** + * {@link SwerveModuleState} describing absolute module rotation and velocity in meters per + * second. + */ + public SwerveModuleState getState() { + return new SwerveModuleState(driveEncoder.getRate(), getAbsoluteHeading()); + } + + /** {@link SwerveModulePosition} describing absolute module rotation and position in meters. */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(driveEncoder.getDistance(), getAbsoluteHeading()); + } + + /** Voltage of the drive motor */ + public double getDriveVoltage() { + return driveMotor.get() * RobotController.getBatteryVoltage(); + } + + /** Voltage of the steer motor */ + public double getSteerVoltage() { + return steerMotor.get() * RobotController.getBatteryVoltage(); + } + + /** + * Constants about this module, like motor IDs, encoder angle offset, and translation from center. + */ + public ModuleConstants getModuleConstants() { + return moduleConstants; + } + + public void log() { + var state = getState(); + + String table = "Module " + moduleConstants.moduleNum + "/"; + SmartDashboard.putNumber( + table + "Steer Degrees", Math.toDegrees(MathUtil.angleModulus(state.angle.getRadians()))); + SmartDashboard.putNumber( + table + "Steer Target Degrees", Math.toDegrees(steerPidController.getSetpoint())); + SmartDashboard.putNumber( + table + "Drive Velocity Feet", Units.metersToFeet(state.speedMetersPerSecond)); + SmartDashboard.putNumber( + table + "Drive Velocity Target Feet", + Units.metersToFeet(desiredState.speedMetersPerSecond)); + SmartDashboard.putNumber(table + "Drive Current", driveCurrentSim); + SmartDashboard.putNumber(table + "Steer Current", steerCurrentSim); + } + + // ----- Simulation + + public void simulationUpdate( + double driveEncoderDist, + double driveEncoderRate, + double driveCurrent, + double steerEncoderDist, + double steerEncoderRate, + double steerCurrent) { + driveEncoderSim.setDistance(driveEncoderDist); + driveEncoderSim.setRate(driveEncoderRate); + this.driveCurrentSim = driveCurrent; + steerEncoderSim.setDistance(steerEncoderDist); + steerEncoderSim.setRate(steerEncoderRate); + this.steerCurrentSim = steerCurrent; + } + + public double getDriveCurrentSim() { + return driveCurrentSim; + } + + public double getSteerCurrentSim() { + return steerCurrentSim; + } +} diff --git a/photonlib-java-examples/swervedriveposeestsim/swerve_module.png b/photonlib-java-examples/swervedriveposeestsim/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-java-examples/swervedriveposeestsim/tag-blue.png b/photonlib-java-examples/swervedriveposeestsim/tag-blue.png new file mode 100644 index 0000000000000000000000000000000000000000..04b9e4f7b7241c94a87249c24fb09c2577b2cf44 GIT binary patch literal 4711 zcmeHL`BPI@6uy8KL`Weqf;O_r;;<=N1zgHf6rvI=C?--RfQSo_M3w-8rcOr_vEWE? z83-NQ0%HxENFvITK^Bq4b{w#p7?~hg1VIc1WRaemK>H80{V6|W&U^2E_nhzC`_6aX zjbIPW-`H@aAwtNQ8sHaxE-pV>SB!(F;Vy4iC{BwiC+C(?cnn+J4 z=~|}*Iqum`u{c(=U9f^de>ycFnNIy>ugiMoaC09SnGruQoMENu zA`=@WLTCLb$omps4;c`Fr;iN*Sp_uephW@?#IrKhM<%vfkah{QZ-KTDv|8~0%c0wZ zmm)UYQXJGh7@ww_-{02TJC;?Qdgiqx<3`cZ>9Kcz$}G=V#>(qYO{c0RC*Ms?d+CeL zQ7I^KhR6<+J+L0(_1!!-dyXoaC<%~x%bK5$jZ*Cs1|x|2$k?*z^;f5I zst3L;gceZl9V%*bP$K4y@uHKF@!vpuaL&ILW)HbLRhT6_SrQE`BD8sL{0bNmT_T_O zfv?9SlM)WXV|KVJ?j2s&A0_cUP>)%t$QYh_JiL&k05JReoARyDR(d2N`fGo{J+|-f z{}QZ&EE9sCAaRg%T}ZXe6l^BFLU9h7Qm1eG!K>IFU)x_qa`geUS%f2(0dDV_J9W2V zY?Gi`h39q+HOUhj)&SA3g*WAGIFh}H9=-$SxJNf_#Xb9I0-u{j*^pxO?YB-)<$| zOld<$22kL+*%0W0>gIgDUNhf=8Qq^8-YJCc9dk{V;hE#Xyhx>#M<&pL0rMhX@9r^R zi!UYzmQVD_1$rpuMwtBS27zAlwMHFEVhQ<{+7It1*V@P`+G2Ejb^2n>|zmaC#*!d)S zjuT4gB_TTK$ndWhx@-yNQAB+<>Hb2*yxYL$KmAI%HqB*uy$l@Dp{e>tIcfUR+}PeY zO<6gqHv#Rhv}A3uA(*dLqS8nbn>#%!?xj(X)F`f)6|`7>g-Ti1Y)j~|A(YTQg*{WJ z|M)w}D$VSegxI*LWq7ILUvVcCS#Fv|H`(c%YPS-dzBl$}%tRB@T`(VA`sm<^d4Du^$0=5e z2F%N??ppv$cecAMM0Z+uzG@&u&7`9{f9|)4_StOrGK;#?D&hSft7$LC)o)Q=*#!5r z%Ps<|y>M6Zpj2@DzXlg8oV=sG?shBf2ud!APp=-`5jBUh;o zi$u*tWgM=&%>Cg)3-~1EH63`Q8FKEt+^ZS7J(i-@1)HJ3mqj_zQ10H(S3<(L9*>o1 zeCKa$k!3-}t{*RAJ!wtDVaL1cHgEI8Rc)szum{%+BC7=tgksDRJe|2*L<_>y@<^;= zEF&FsVU}7>7{WRzDO9?nMwpGQZ;cR3k?V2R)FvUDfF;P_){{2SOesSk88iX=j4v5f z%hy|=NpcN@$PYv`7S7Ir#FBul@^bi+R1B@enULbCFyzk=cIJTR0$WA@F1VkINZNb` z+)uW?xr6ml=>n=MeI$^mMYzg6BagNQqLi->EpD>{rM7F7-G)r)I-}yPFl~7(CRn}< zUp{=3_yN@G^BK==*+kYL$9>!UyWJwMsaMJB_CSA0Pn(>2n?2CGRy-k58 zhlpkmzZW0+cmV4{$5pCa`C=}mG9&%>nvcuk2m3M!ph+q_C6g?Ig4G@(v`e5pN@@?v k+Jo}Hd{91lsCH^;si4Yp)^TGd{5gcE+i8BKzRa_K0jU-?_5c6? literal 0 HcmV?d00001 diff --git a/photonlib-java-examples/swervedriveposeestsim/tag-green.png b/photonlib-java-examples/swervedriveposeestsim/tag-green.png new file mode 100644 index 0000000000000000000000000000000000000000..63029fcf205d2a1b52eb450aa1e26fbdfd310586 GIT binary patch literal 4698 zcmeHLYg3a~5I*5jKx+ge)e0&gii(5a6%bGmxikcdU<|@&07sk_2r+;_t4$FlTolv+ zLJ^(TkqjkV6#{Ze#3)q>+DXCEM4%CpMhuogNfFWR2{8QwZ9nBhX5Vx6d3T@Pb9T>p zlk<77|Dpve79fNc1qS%;N63JQkm1LsAHj@=-}q$^1lE3kA5_z0t%iY7oHxZAq56V_ z@1l)iY?c%d!9s}GiGKvk^`Eg3vUnBf>m8nUSR?biL^`;P9jzprnN`{yT`j)3bji&n zq+iLl$5s<7?d+31ioFEuqqoH*`|qAf-5pZ&g1Y~Lf+`R3G$R_q-!ONX&=hZlhZum`}dj;CRSR;k<)Syudg#^ zMg|S3ckPMD{j_OwwMHcwjV7fD%et%czNsd}1}T(}>=Q3GecN=tQT_aSkg^X$R}6$a zzCO8*B{d_jRUrDed|F9}m>}}eP)5XZ12`6Ft}cQgI47tvo0S!$hj29T^DE_BIH0k0 z?DK*%!*QMRHcSt7MBy`|V{6f%C;HG8Z{OLLcVn3zc7j zv3Vqx$^m3me_o3^D-?(}yN9)?NZ^S=tM|g55+CNKp&jj7fBlSOuC*eV2GAzy$5qjT+iMYgD&@99f?WkOReAidxIABHq-y7ttAA-E!fn zbglYLiJC;NJAXbQEoimdb%}Q5xpMsbzsfC`qU9DQn*v;vzShouV#31k?~b;TOpx=2 zXC+&64DWMcYNyt+ieq@6<4RwvT*0yab6tK4qIGzJ?ZZp&Ur>Sb4YexS98@^ao|;zMBbpWans#s)jyrR{7tah0eJ zE|%^>HAk%&WnI}JwZb-YU93slr^lNh$vBZ4cMwhrRx@^u!R@L0N$T54nkbQ0vS=A4lJO@&t4C_q4k_yyE z`*B4!o$FQG;z~XArOviZoCL28V&}K3WyuYPLB(xv)FlE6JV(%ltF%}wV`4=S#&qJ> zxyDseDNq|yie+ltSGH1l&#_FQ@*eB@iUmW3%~qK+RL~&VdB2r)P#mtDff$@RU%b}! zBC0~;j^Ej#&}4x8Gal2-BIDNLTqV^-_dqPu;4WydNLYm-_Bu!;Zit~&-UQsb+5S0# zDLfA02dkh3qrr&sRA9EJ%@|&!xsCS$h<*cS&}=O*r`~^&{3)2?XQf4c0)5BPW}q0h zjwn+@8LS```@o|?M7h&7jv@QpZk{Ssk>CkvqxduNR>!4X*EoVVko70yzYA?02Kq)5 zGYqaMC#tg5D_Ij`WrLGvTR*%#Hk~F+pV=Vqd$epN^cE#qd}kw#XQ2OB(0?z%_SAne l>QkWK1^;Rx47a6?FD6iOngjQS4#WRJC~#k}Z;cN<^KVFxHsJsO literal 0 HcmV?d00001