Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51: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

45
photonlib-java-examples/simaimandrange/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);
}
}

129
photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java Normal file
View File

@@ -0,0 +1,129 @@
/*
* 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.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
import frc.robot.sim.DrivetrainSim;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonUtils;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
// 2020 High goal target height above ground
public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
// Constants about how your camera is mounted to the robot
public static final double CAMERA_PITCH_RADIANS =
Units.degreesToRadians(15); // Angle "up" from horizontal
public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor
// How far from the target we want to be
final double GOAL_RANGE_METERS = Units.feetToMeters(10);
// Change this to match the name of your camera
PhotonCamera camera = new PhotonCamera("photonvision");
// PID constants should be tuned per robot
final double LINEAR_P = 2.0;
final double LINEAR_D = 0.0;
PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
final double ANGULAR_P = 0.03;
final double ANGULAR_D = 0.003;
PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
XboxController xboxController = new XboxController(0);
// Drive motors
PWMVictorSPX leftMotor = new PWMVictorSPX(0);
PWMVictorSPX rightMotor = new PWMVictorSPX(1);
DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
@Override
public void teleopPeriodic() {
double forwardSpeed;
double rotationSpeed;
if (xboxController.getAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
var result = camera.getLatestResult();
if (result.hasTargets()) {
// First calculate range
double range =
PhotonUtils.calculateDistanceToTargetMeters(
CAMERA_HEIGHT_METERS,
TARGET_HEIGHT_METERS,
CAMERA_PITCH_RADIANS,
Units.degreesToRadians(result.getBestTarget().getPitch()));
// Use this range as the measurement we give to the PID controller.
// -1.0 required to ensure positive PID controller effort _increases_ range
forwardSpeed = -forwardController.calculate(range, GOAL_RANGE_METERS);
// Also calculate angular power
// -1.0 required to ensure positive PID controller effort _increases_ yaw
rotationSpeed = -turnController.calculate(result.getBestTarget().getYaw(), 0);
} else {
// If we have no targets, stay still.
forwardSpeed = 0;
rotationSpeed = 0;
}
} else {
// Manual Driver Mode
forwardSpeed = -xboxController.getRightY();
rotationSpeed = xboxController.getLeftX();
}
// Use our forward/turn speeds to control the drivetrain
drive.arcadeDrive(forwardSpeed, rotationSpeed);
}
////////////////////////////////////////////////////
// Simulation support
DrivetrainSim dtSim;
@Override
public void simulationInit() {
dtSim = new DrivetrainSim();
}
@Override
public void simulationPeriodic() {
dtSim.update();
}
}

153
photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java Normal file
View File

@@ -0,0 +1,153 @@
/*
* 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.sim;
import edu.wpi.first.math.geometry.Pose2d;
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.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.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.PWMSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import org.photonvision.SimVisionSystem;
import org.photonvision.SimVisionTarget;
/**
* 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 Motor Controllers
PWMSim leftLeader = new PWMSim(0);
PWMSim rightLeader = new PWMSim(1);
// 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, 1.0);
DifferentialDrivetrainSim drivetrainSimulator =
new DifferentialDrivetrainSim(
drivetrainSystem,
DCMotor.getCIM(2),
8,
Units.feetToMeters(2.0),
Units.inchesToMeters(6.0 / 2.0),
null);
// Simulated Vision System.
// Configure these to match your PhotonVision Camera,
// pipeline, and LED setup.
double camDiagFOV = 170.0; // degrees - assume wide-angle camera
double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees
double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
double maxLEDRange = 20; // meters
int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels
double minTargetArea = 10; // square pixels
SimVisionSystem simVision =
new SimVisionSystem(
"photonvision",
camDiagFOV,
new Transform3d(
new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)),
maxLEDRange,
camResolutionWidth,
camResolutionHeight,
minTargetArea);
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 208
double targetWidth = Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 197
double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
// pages 4 and 5
double tgtXPos = Units.feetToMeters(54);
double tgtYPos =
Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0);
Pose3d farTargetPose =
new Pose3d(
new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS),
new Rotation3d(0.0, 0.0, 0.0));
Field2d field = new Field2d();
public DrivetrainSim() {
simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1));
SmartDashboard.putData("Field", field);
}
/**
* 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()) {
leftMotorCmd = leftLeader.getSpeed();
rightMotorCmd = rightLeader.getSpeed();
}
drivetrainSimulator.setInputs(
leftMotorCmd * RobotController.getInputVoltage(),
-rightMotorCmd * RobotController.getInputVoltage());
drivetrainSimulator.update(0.02);
// Update PhotonVision based on our new robot position.
simVision.processFrame(drivetrainSimulator.getPose());
field.setRobotPose(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);
}
}