mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Update to wpilib 2023 beta 7 (#607)
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
@@ -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.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 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 {
|
||||
// Constants such as camera and target height stored. Change per robot and goal!
|
||||
final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
|
||||
final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
|
||||
|
||||
// Angle between horizontal and the camera.
|
||||
final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
|
||||
|
||||
// How far from the target we want to be
|
||||
final double GOAL_RANGE_METERS = Units.feetToMeters(3);
|
||||
|
||||
// Change this to match the name of your camera
|
||||
PhotonCamera camera = new PhotonCamera("photonvision");
|
||||
|
||||
// PID constants should be tuned per robot
|
||||
final double P_GAIN = 0.1;
|
||||
final double D_GAIN = 0.0;
|
||||
PIDController controller = new PIDController(P_GAIN, 0, D_GAIN);
|
||||
|
||||
XboxController xboxController;
|
||||
|
||||
// Drive motors
|
||||
PWMVictorSPX leftMotor = new PWMVictorSPX(0);
|
||||
PWMVictorSPX rightMotor = new PWMVictorSPX(1);
|
||||
DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
xboxController = new XboxController(0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
double forwardSpeed;
|
||||
double rotationSpeed = xboxController.getLeftX();
|
||||
|
||||
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 = -controller.calculate(range, GOAL_RANGE_METERS);
|
||||
} else {
|
||||
// If we have no targets, stay still.
|
||||
forwardSpeed = 0;
|
||||
}
|
||||
} else {
|
||||
// Manual Driver Mode
|
||||
forwardSpeed = -xboxController.getRightY();
|
||||
}
|
||||
|
||||
// Use our forward/turn speeds to control the drivetrain
|
||||
drive.arcadeDrive(forwardSpeed, rotationSpeed);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user