diff --git a/.gitignore b/.gitignore index 21cd24014..4f698d642 100644 --- a/.gitignore +++ b/.gitignore @@ -104,15 +104,15 @@ fabric.properties # Android studio 3.1+ serialized cache file .idea/caches/build_file_checksums.ser - -photon-server/.gradle -photon-server/target -photon-server/src/main/java/META-INF -photon-server/.settings -photon-server/.classpath -photon-server/.project -photon-server/settings -photon-server/dependency-reduced-pom.xml +# Temporary build files +**/.gradle +**/target +**/src/main/java/META-INF +**/.settings +**/.classpath +**/.project +**/settings +**/dependency-reduced-pom.xml # photon-server/photon-vision.iml New client/photon-client/* diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 379e268a3..b19336013 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -5,7 +5,6 @@ plugins { id 'google-test-test-suite' id 'edu.wpi.first.NativeUtils' version '2020.10.1' id 'edu.wpi.first.GradleJni' version '0.10.1' - id 'edu.wpi.first.GradleVsCode' version '0.12.0' } repositories { diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 75473cf58..8d8f60228 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,9 +1,42 @@ -apply plugin: 'java' +import edu.wpi.first.toolchain.NativePlatforms -String wpilibVersion = "2021.1.2" -String opencvVersion = "3.4.7-5" -String ejmlVersion = "0.38" -String jacksonVersion = "2.10.0" +plugins { + id 'edu.wpi.first.NativeUtils' version '2021.1.1' +} + +apply plugin: 'java' +apply plugin: 'cpp' +apply plugin: 'edu.wpi.first.NativeUtils' + +// This adds all of the WPILib dependencies. +nativeUtils.addWpiNativeUtils() +nativeUtils { + wpi { + configureDependencies { + wpiVersion = "2021.1.2" + opencvVersion = "3.4.7-5" + wpimathVersion = "2021.1.2" + } + } + dependencyConfigs { + gui { + groupId = "edu.wpi.first.halsim" + artifactId = "halsim_gui" + headerClassifier = "headers" + sourceClassifier = "sources" + ext = "zip" + version = "2021.1.2" + sharedPlatforms << "osxx86-64" << "linuxx86-64" << "windowsx86-64" + } + } + combinedDependencyConfigs { + halsim_gui { + libraryName = "halsim_gui" + targetPlatforms << "osxx86-64" << "linuxx86-64" << "windowsx86-64" + dependencies << "gui_shared" + } + } +} repositories { maven { @@ -11,25 +44,102 @@ repositories { } } +ext { + exampleFile = new File("$projectDir/src/main/java/org/photonlib/examples/examples.json") +} + dependencies { implementation project(':photon-lib') implementation project(':photon-targeting') - implementation "edu.wpi.first.wpilibj:wpilibj-java:${wpilibVersion}" - implementation "edu.wpi.first.wpimath:wpimath-java:${wpilibVersion}" - implementation "edu.wpi.first.ntcore:ntcore-java:${wpilibVersion}" - implementation "edu.wpi.first.wpiutil:wpiutil-java:${wpilibVersion}" - implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}" - implementation "edu.wpi.first.cscore:cscore-java:${wpilibVersion}" - implementation "edu.wpi.first.cameraserver:cameraserver-java:${wpilibVersion}" - implementation "edu.wpi.first.hal:hal-java:${wpilibVersion}" - implementation "org.ejml:ejml-simple:${ejmlVersion}" - implementation "com.fasterxml.jackson.core:jackson-annotations:${jacksonVersion}" - implementation "com.fasterxml.jackson.core:jackson-core:${jacksonVersion}" - implementation "com.fasterxml.jackson.core:jackson-databind:${jacksonVersion}" + implementation 'edu.wpi.first.cscore:cscore-java:2021.+' + implementation 'edu.wpi.first.cameraserver:cameraserver-java:2021.+' + implementation 'edu.wpi.first.ntcore:ntcore-java:2021.+' + implementation 'edu.wpi.first.wpilibj:wpilibj-java:2021.+' + implementation 'edu.wpi.first.wpiutil:wpiutil-java:2021.+' + implementation 'edu.wpi.first.wpimath:wpimath-java:2021.+' + implementation 'edu.wpi.first.hal:hal-java:2021.+' + implementation "org.ejml:ejml-simple:0.38" + implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0" + implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" + implementation "com.fasterxml.jackson.core:jackson-databind:2.10.0" + implementation 'edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:3.4.7-3' } -ext { - exampleDirectory = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/") - exampleFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/examples/examples.json") -} \ No newline at end of file +model { + components { + all { + nativeUtils.useAllPlatforms(it) + } + dev(NativeExecutableSpec) { + targetBuildTypes 'debug' + sources { + cpp { + source { + srcDirs 'src/dev/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/dev/native/include' + } + } + } + binaries.all { binary -> + nativeUtils.useRequiredLibrary(binary, 'wpilib_executable_shared') + nativeUtils.useRequiredLibrary(binary, 'opencv_shared') + nativeUtils.useRequiredLibrary(binary, 'halsim_gui') + } + } + } + tasks { + def found = false + $.components.each { + if (it in NativeExecutableSpec && it.name == "dev") { + it.binaries.each { + if (found) + return + + def arch = it.targetPlatform.name + if (arch == NativePlatforms.desktop) { + found = true + def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib' + + def doFirstTask = { task -> + def extensions = '' + it.tasks.install.installDirectory.get().getAsFile().eachFileRecurse { + def name = it.name + if (!(name.endsWith('.dll') || name.endsWith('.so') || name.endsWith('.dylib'))) { + return + } + def file = it + if (name.startsWith("halsim_gui") || name.startsWith("libhalsim_gui".toString())) { + extensions += file.absolutePath + File.pathSeparator + } + } + if (extensions != '') { + task.environment 'HALSIM_EXTENSIONS', extensions + } + } + + new groovy.json.JsonSlurper().parseText(exampleFile.text).each { entry -> + project.tasks.create("run${entry.foldername}", JavaExec) { run -> + main = "org.photonlib.examples." + entry.foldername + ".Main" + classpath = sourceSets.main.runtimeClasspath + run.dependsOn it.tasks.install + run.systemProperty 'java.library.path', filePath + run.environment 'LD_LIBRARY_PATH', filePath + run.workingDir filePath + doFirst { doFirstTask(run) } + + if (org.gradle.internal.os.OperatingSystem.current().isMacOsX()) { + run.jvmArgs = ['-XstartOnFirstThread'] + } + } + } + + } + } + } + } + } +} diff --git a/photonlib-java-examples/settings.gradle b/photonlib-java-examples/settings.gradle deleted file mode 100644 index fe6bf76ca..000000000 --- a/photonlib-java-examples/settings.gradle +++ /dev/null @@ -1 +0,0 @@ -rootproject.name = 'photonlib-java-examples' \ No newline at end of file diff --git a/photonlib-java-examples/src/dev/native/cpp/main.cpp b/photonlib-java-examples/src/dev/native/cpp/main.cpp new file mode 100644 index 000000000..70059c2f2 --- /dev/null +++ b/photonlib-java-examples/src/dev/native/cpp/main.cpp @@ -0,0 +1,8 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +int main() {} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json index 21570e837..fe36863dd 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/examples.json @@ -34,5 +34,17 @@ "packagetoreplace": null, "dependencies": [], "foldername": "aimandrange" + }, + { + "name": "SimPoseEstimation", + "description": "Integrate 3D vision processing mode results into estimation of robot pose on the field. Includes simulation support.", + "tags": [], + "gradlebase": "java", + "language": "java", + "commandversion": 1, + "mainclass": "Main", + "packagetoreplace": null, + "dependencies": [], + "foldername": "simposeest" } ] \ No newline at end of file diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java new file mode 100644 index 000000000..6eb052216 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/Main.java @@ -0,0 +1,39 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest; + +import edu.wpi.first.wpilibj.RobotBase; +import org.photonlib.examples.simposeest.robot.Robot; + +/** +* 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/src/main/java/org/photonlib/examples/simposeest/resources/blue_square.png b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/blue_square.png new file mode 100644 index 000000000..63b52bdd4 Binary files /dev/null and b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/blue_square.png differ diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/green_square.png b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/green_square.png new file mode 100644 index 000000000..d70576a43 Binary files /dev/null and b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/green_square.png differ diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/red_square.png b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/red_square.png new file mode 100644 index 000000000..733814f18 Binary files /dev/null and b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/resources/red_square.png differ diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/AutoController.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/AutoController.java new file mode 100644 index 000000000..738b6bbc5 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/AutoController.java @@ -0,0 +1,104 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +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/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java new file mode 100644 index 000000000..243862f63 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java @@ -0,0 +1,96 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.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; + + ////////////////////////////////////////////////////////////////// + // 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 Transform2d kCameraToRobot = + new Transform2d( + new Translation2d(-0.25, 0), // in meters + new Rotation2d()); + + // 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 + public static final double targetHeightAboveGround = 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 Pose2d kFarTargetPose = + new Pose2d(new Translation2d(kFarTgtXPos, kFarTgtYPos), new Rotation2d(0.0)); + + public static final SimVisionTarget kFarTarget = + new SimVisionTarget(kFarTargetPose, targetHeightAboveGround, targetWidth, targetHeight); +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java new file mode 100644 index 000000000..cbe1d45bd --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Drivetrain.java @@ -0,0 +1,132 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; + +/** +* 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); + + SpeedControllerGroup leftGroup = new SpeedControllerGroup(leftLeader, leftFollower); + SpeedControllerGroup rightGroup = new SpeedControllerGroup(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 + DrivetrainPoseEstimator poseEst = new DrivetrainPoseEstimator(); + + // 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); + } + + /** + * 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( + new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()), + 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); + } + + /** @return The current best-guess at drivetrain Pose on the field. */ + public Pose2d getCtrlsPoseEstimate() { + return poseEst.getPoseEst(); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java new file mode 100644 index 000000000..3b0510ac2 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java @@ -0,0 +1,106 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.estimator.DifferentialDrivePoseEstimator; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N5; +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 = + new DifferentialDrivePoseEstimator( + gyro.getRotation2d(), + new Pose2d(), + stateStdDevs, + localMeasurementStdDevs, + visionMeasurementStdDevs); + + public DrivetrainPoseEstimator() {} + + /** + * 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( + DifferentialDriveWheelSpeeds actWheelSpeeds, double leftDist, double rightDist) { + + m_poseEstimator.update(gyro.getRotation2d(), actWheelSpeeds, leftDist, rightDist); + + var res = cam.getLatestResult(); + if (res.hasTargets()) { + double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis(); + Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget(); + Pose2d camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse()); + m_poseEstimator.addVisionMeasurement( + camPose.transformBy(Constants.kCameraToRobot), 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). + * + * @param pose + */ + public void resetToPose(Pose2d pose) { + m_poseEstimator.resetPosition(pose, gyro.getRotation2d()); + } + + /** @return The current best-guess at drivetrain position on the field. */ + public Pose2d getPoseEst() { + return m_poseEstimator.getEstimatedPosition(); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java new file mode 100644 index 000000000..c2e151cce --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/OperatorInterface.java @@ -0,0 +1,51 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.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.getY(GenericHID.Hand.kLeft)) * 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.getX(GenericHID.Hand.kRight)) * Constants.kMaxAngularSpeed; + } + + public boolean getSimKickCmd() { + return opCtrl.getXButtonPressed(); + } +} diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java new file mode 100644 index 000000000..02f9192b3 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/PoseTelemetry.java @@ -0,0 +1,55 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.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/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java new file mode 100644 index 000000000..fa94b5096 --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Robot.java @@ -0,0 +1,85 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.robot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import org.photonlib.examples.simposeest.sim.DrivetrainSim; + +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/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java new file mode 100644 index 000000000..9a0811e1b --- /dev/null +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java @@ -0,0 +1,168 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonlib.examples.simposeest.sim; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +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 edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.numbers.N2; +import org.photonlib.examples.simposeest.robot.Constants; +import org.photonvision.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, + camPitch, + Constants.kCameraToRobot, + camHeightOffGround, + 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.getInstance().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); + } +}