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

Update to wpilib 2023 beta 7 (#607)

Browse Source 
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
shueja-personal
2022-12-16 17:05:23 -08:00
committed by GitHub
parent da1aabae3a
commit bb63af601d
198 changed files with 6339 additions and 4525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,162 @@
# 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

6
photonlib-java-examples/simposeest/.wpilib/wpilib_preferences.json Normal file
View File

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

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

@@ -0,0 +1,24 @@
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.

94
photonlib-java-examples/simposeest/build.gradle Normal file
View File

@@ -0,0 +1,94 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
}
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'
}

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

@@ -0,0 +1,29 @@
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
}
}
}

102
photonlib-java-examples/simposeest/simgui-ds.json Normal file
View File

@@ -0,0 +1,102 @@
{
"Keyboard 0 Settings": {
"window": {
"visible": true
}
},
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"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"
}
]
}

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

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

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

@@ -0,0 +1,3 @@
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.

108
photonlib-java-examples/simposeest/src/main/java/frc/robot/AutoController.java Normal file
View File

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

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

@@ -0,0 +1,110 @@
/*
* MIT License
*
* Copyright (c) 2022 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.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);
}

137
photonlib-java-examples/simposeest/src/main/java/frc/robot/Drivetrain.java Normal file
View File

@@ -0,0 +1,137 @@
/*
* MIT License
*
* Copyright (c) 2022 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();
}
}

107
photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainPoseEstimator.java Normal file
View File

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

170
photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java Normal file
View File

@@ -0,0 +1,170 @@
/*
* MIT License
*
* Copyright (c) 2022 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.SimVisionSystem;
/**
* Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated
* PhotonVision system and one vision target.
*
* <p>This class and its methods are only relevant during simulation. While on the real robot, the
* real motors/sensors/physics are used instead.
*/
public class DrivetrainSim {
// Simulated Sensors
AnalogGyroSim gyroSim = new AnalogGyroSim(Constants.kGyroPin);
EncoderSim leftEncoderSim = EncoderSim.createForChannel(Constants.kDtLeftEncoderPinA);
EncoderSim rightEncoderSim = EncoderSim.createForChannel(Constants.kDtRightEncoderPinA);
// Simulated Motor Controllers
PWMSim leftLeader = new PWMSim(Constants.kDtLeftLeaderPin);
PWMSim leftFollower = new PWMSim(Constants.kDtLeftFollowerPin);
PWMSim rightLeader = new PWMSim(Constants.kDtRightLeaderPin);
PWMSim rightFollower = new PWMSim(Constants.kDtRightFollowerPin);
// Simulation Physics
// Configure these to match your drivetrain's physical dimensions
// and characterization results.
LinearSystem<N2, N2, N2> drivetrainSystem =
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim(
drivetrainSystem,
DCMotor.getCIM(2),
8,
Constants.kTrackWidth,
Constants.kWheelRadius,
null);
// Simulated Vision System.
// Configure these to match your PhotonVision Camera,
// pipeline, and LED setup.
double camDiagFOV = 75.0; // degrees
double camPitch = 15.0; // degrees
double camHeightOffGround = 0.85; // meters
double maxLEDRange = 20; // meters
int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels
double minTargetArea = 10; // square pixels
SimVisionSystem simVision =
new SimVisionSystem(
Constants.kCamName,
camDiagFOV,
Constants.kCameraToRobot,
maxLEDRange,
camResolutionWidth,
camResolutionHeight,
minTargetArea);
public DrivetrainSim() {
simVision.addSimVisionTarget(Constants.kFarTarget);
}
/**
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
* physics forward by a single 20ms step.
*/
public void update() {
double leftMotorCmd = 0;
double rightMotorCmd = 0;
if (DriverStation.isEnabled() && !RobotController.isBrownedOut()) {
// If the motor controllers are enabled...
// Roughly model the effect of leader and follower motor pushing on the same
// gearbox.
// Note if the software is incorrect and drives them against each other, speed
// will be
// roughly matching the physical situation, but current draw will _not_ be
// accurate.
leftMotorCmd = (leftLeader.getSpeed() + leftFollower.getSpeed()) / 2.0;
rightMotorCmd = (rightLeader.getSpeed() + rightFollower.getSpeed()) / 2.0;
}
// Update the physics simulation
drivetrainSimulator.setInputs(
leftMotorCmd * RobotController.getInputVoltage(),
-rightMotorCmd * RobotController.getInputVoltage());
drivetrainSimulator.update(0.02);
// Update our sensors based on the simulated motion of the robot
leftEncoderSim.setDistance((drivetrainSimulator.getLeftPositionMeters()));
leftEncoderSim.setRate((drivetrainSimulator.getLeftVelocityMetersPerSecond()));
rightEncoderSim.setDistance((drivetrainSimulator.getRightPositionMeters()));
rightEncoderSim.setRate((drivetrainSimulator.getRightVelocityMetersPerSecond()));
gyroSim.setAngle(
-drivetrainSimulator
.getHeading()
.getDegrees()); // Gyros have an inverted reference frame for
// angles, so multiply by -1.0;
// Update PhotonVision based on our new robot position.
simVision.processFrame(drivetrainSimulator.getPose());
}
/**
* Resets the simulation back to a pre-defined pose Useful to simulate the action of placing the
* robot onto a specific spot in the field (IE, at the start of each match).
*
* @param pose
*/
public void resetPose(Pose2d pose) {
drivetrainSimulator.setPose(pose);
}
/** @return The simulated robot's position, in meters. */
public Pose2d getCurPose() {
return drivetrainSimulator.getPose();
}
/**
* For testing purposes only! Applies an unmodeled, undetected offset to the pose Similar to if
* you magically kicked your robot to the side in a way the encoders and gyro didn't measure.
*
* <p>This distrubance should be corrected for once a vision target is in view.
*/
public void applyKick() {
Pose2d newPose =
drivetrainSimulator
.getPose()
.transformBy(new Transform2d(new Translation2d(0, 0.5), new Rotation2d()));
drivetrainSimulator.setPose(newPose);
}
}

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

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

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

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

61
photonlib-java-examples/simposeest/src/main/java/frc/robot/PoseTelemetry.java Normal file
View File

@@ -0,0 +1,61 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package frc.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/** Reports our expected, desired, and actual poses to dashboards */
public class PoseTelemetry {
Field2d field = new Field2d();
Pose2d actPose = new Pose2d();
Pose2d desPose = new Pose2d();
Pose2d estPose = new Pose2d();
public PoseTelemetry() {
SmartDashboard.putData("Field", field);
update();
}
public void update() {
field.getObject("DesPose").setPose(desPose);
field.getObject("ActPose").setPose(actPose);
field.getObject("Robot").setPose(estPose);
}
public void setActualPose(Pose2d in) {
actPose = in;
}
public void setDesiredPose(Pose2d in) {
desPose = in;
}
public void setEstimatedPose(Pose2d in) {
estPose = in;
}
}

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

@@ -0,0 +1,90 @@
/*
* MIT License
*
* Copyright (c) 2022 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);
}
}