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

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