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

Upgrade to Gradle 7.2 and WPILib 2022 (#316)

Browse Source
This commit is contained in:
Tyler Veness
2021-11-21 17:22:56 -08:00
committed by GitHub
parent ffe34f00fe
commit 5ca39e7f84
316 changed files with 671 additions and 1019 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
photonlib-cpp-examples/src/main/cpp/examples/aimandrange/cpp/Robot.cpp
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -26,24 +26,24 @@ void Robot::TeleopPeriodic() {
if (xboxController.GetAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
const auto &result = camera.GetLatestResult();
const auto& result = camera.GetLatestResult();
if (result.HasTargets()) {
// First calculate range
units::meter_t range =
photonlib::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{result.GetBestTarget().GetPitch()});
units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{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 = -1.0* forwardController.Calculate(
range.to<double>(), GOAL_RANGE_METERS.to<double>());
// -1.0 required to ensure positive PID controller effort _increases_
// range
forwardSpeed = -forwardController.Calculate(range.value(),
GOAL_RANGE_METERS.value());
// Also calculate angular power
// -1.0 required to ensure positive PID controller effort _increases_ yaw
rotationSpeed = -1.0 * turnController.Calculate(
result.GetBestTarget().GetYaw(), 0);
rotationSpeed =
-turnController.Calculate(result.GetBestTarget().GetYaw(), 0);
} else {
// If we have no targets, stay still.
forwardSpeed = 0;
@@ -51,10 +51,8 @@ void Robot::TeleopPeriodic() {
}
} else {
// Manual Driver Mode
forwardSpeed =
-1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
rotationSpeed =
xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
forwardSpeed = -xboxController.GetRightY();
rotationSpeed = xboxController.GetLeftX();
}
// Use our forward/turn speeds to control the drivetrain

10
photonlib-cpp-examples/src/main/cpp/examples/aimandrange/include/Robot.h
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -19,19 +19,19 @@
#include <photonlib/PhotonCamera.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
class Robot : public frc::TimedRobot {
public:
public:
void TeleopPeriodic() override;
private:
private:
// Constants such as camera and target height stored. Change per robot and
// goal!
const units::meter_t CAMERA_HEIGHT = 24_in;

12
photonlib-cpp-examples/src/main/cpp/examples/aimattarget/cpp/Robot.cpp
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -20,8 +20,7 @@
#include <photonlib/PhotonUtils.h>
void Robot::TeleopPeriodic() {
double forwardSpeed =
-1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
double forwardSpeed = -xboxController.GetRightY();
double rotationSpeed;
if (xboxController.GetAButton()) {
@@ -31,15 +30,14 @@ void Robot::TeleopPeriodic() {
if (result.HasTargets()) {
// Rotation speed is the output of the PID controller
rotationSpeed = -1.0 * controller.Calculate(result.GetBestTarget().GetYaw(), 0);
rotationSpeed = -controller.Calculate(result.GetBestTarget().GetYaw(), 0);
} else {
// If we have no targets, stay still.
rotationSpeed = 0;
}
} else {
// Manual Driver Mode
rotationSpeed =
xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
rotationSpeed = xboxController.GetLeftX();
}
// Use our forward/turn speeds to control the drivetrain

10
photonlib-cpp-examples/src/main/cpp/examples/aimattarget/include/Robot.h
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -19,19 +19,19 @@
#include <photonlib/PhotonCamera.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
class Robot : public frc::TimedRobot {
public:
public:
void TeleopPeriodic() override;
private:
private:
// Change this to match the name of your camera
photonlib::PhotonCamera camera{"photonvision"};
// PID constants should be tuned per robot

14
photonlib-cpp-examples/src/main/cpp/examples/getinrange/cpp/Robot.cpp
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -21,8 +21,7 @@
void Robot::TeleopPeriodic() {
double forwardSpeed;
double rotationSpeed =
xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
double rotationSpeed = xboxController.GetLeftX();
if (xboxController.GetAButton()) {
// Vision-alignment mode
@@ -36,16 +35,15 @@ void Robot::TeleopPeriodic() {
units::degree_t{result.GetBestTarget().GetPitch()});
// Use this range as the measurement we give to the PID controller.
forwardSpeed = -1.0 * controller.Calculate(range.to<double>(),
GOAL_RANGE_METERS.to<double>());
forwardSpeed =
-controller.Calculate(range.value(), GOAL_RANGE_METERS.value());
} else {
// If we have no targets, stay still.
forwardSpeed = 0;
}
} else {
// Manual Driver Mode
forwardSpeed =
-1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
forwardSpeed = -xboxController.GetRightY();
}
// Use our forward/turn speeds to control the drivetrain

10
photonlib-cpp-examples/src/main/cpp/examples/getinrange/include/Robot.h
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -19,19 +19,19 @@
#include <photonlib/PhotonCamera.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
class Robot : public frc::TimedRobot {
public:
public:
void TeleopPeriodic() override;
private:
private:
// Constants such as camera and target height stored. Change per robot and
// goal!
const units::meter_t CAMERA_HEIGHT = 24_in;

36
photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -26,24 +26,24 @@ void Robot::TeleopPeriodic() {
if (xboxController.GetAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
const auto &result = camera.GetLatestResult();
const auto& result = camera.GetLatestResult();
if (result.HasTargets()) {
// First calculate range
units::meter_t range =
photonlib::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{result.GetBestTarget().GetPitch()});
units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{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 = -1.0* forwardController.Calculate(
range.to<double>(), GOAL_RANGE_METERS.to<double>());
// -1.0 required to ensure positive PID controller effort _increases_
// range
forwardSpeed = -forwardController.Calculate(range.value(),
GOAL_RANGE_METERS.value());
// Also calculate angular power
// -1.0 required to ensure positive PID controller effort _increases_ yaw
rotationSpeed = -1.0 * turnController.Calculate(
result.GetBestTarget().GetYaw(), 0);
rotationSpeed =
-turnController.Calculate(result.GetBestTarget().GetYaw(), 0);
} else {
// If we have no targets, stay still.
forwardSpeed = 0;
@@ -51,22 +51,16 @@ void Robot::TeleopPeriodic() {
}
} else {
// Manual Driver Mode
forwardSpeed =
-1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
rotationSpeed =
xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
forwardSpeed = -xboxController.GetRightY();
rotationSpeed = xboxController.GetLeftX();
}
// Use our forward/turn speeds to control the drivetrain
drive.ArcadeDrive(forwardSpeed, rotationSpeed);
}
void Robot::SimulationPeriodic() {
dtSim.update();
}
void Robot::SimulationPeriodic() { dtSim.update(); }
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

86
photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp
View File

@@ -1,43 +1,43 @@
/**
* Copyright (C) 2018-2020 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 <https://www.gnu.org/licenses/>.
*/
#include "DrivetrainSim.h"
/**
* Perform all periodic drivetrain simulation related tasks to advance our simulation of robot
* physics forward by a single 20ms step.
*/
void DrivetrainSim::update(){
double leftMotorCmd = 0;
double rightMotorCmd = 0;
if (frc::DriverStation::GetInstance().IsEnabled() && !frc::RobotController::IsBrownedOut()) {
leftMotorCmd = leftLeader.GetSpeed();
rightMotorCmd = rightLeader.GetSpeed();
}
m_drivetrainSimulator.SetInputs(
units::volt_t(leftMotorCmd * frc::RobotController::GetInputVoltage()),
units::volt_t(-rightMotorCmd * frc::RobotController::GetInputVoltage()) );
m_drivetrainSimulator.Update(20_ms);
// Update PhotonVision based on our new robot position.
simVision.ProcessFrame(m_drivetrainSimulator.GetPose());
field.SetRobotPose(m_drivetrainSimulator.GetPose());
}
/*
* 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 <https://www.gnu.org/licenses/>.
*/
#include "DrivetrainSim.h"
/**
* Perform all periodic drivetrain simulation related tasks to advance our
* simulation of robot physics forward by a single 20ms step.
*/
void DrivetrainSim::update() {
double leftMotorCmd = 0;
double rightMotorCmd = 0;
if (frc::DriverStation::IsEnabled() &&
!frc::RobotController::IsBrownedOut()) {
leftMotorCmd = leftLeader.GetSpeed();
rightMotorCmd = rightLeader.GetSpeed();
}
m_drivetrainSimulator.SetInputs(
units::volt_t(leftMotorCmd * frc::RobotController::GetInputVoltage()),
units::volt_t(-rightMotorCmd * frc::RobotController::GetInputVoltage()));
m_drivetrainSimulator.Update(20_ms);
// Update PhotonVision based on our new robot position.
simVision.ProcessFrame(m_drivetrainSimulator.GetPose());
field.SetRobotPose(m_drivetrainSimulator.GetPose());
}

212
photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h
View File

@@ -1,107 +1,105 @@
/**
* Copyright (C) 2018-2020 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 <https://www.gnu.org/licenses/>.
*/
#include <frc/geometry/Pose2d.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/geometry/Translation2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/simulation/PWMSim.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/time.h>
#include <photonlib/SimVisionSystem.h>
#include <frc/DriverStation.h>
#include <frc/RobotController.h>
#pragma once
class DrivetrainSim {
public:
DrivetrainSim(){
simVision.AddSimVisionTarget(photonlib::SimVisionTarget(farTargetPose, 81.91_in, targetWidth, targetHeight));
frc::SmartDashboard::PutData("Field", &field);
}
void update();
private:
// Simulated Motor Controllers
frc::sim::PWMSim leftLeader {0};
frc::sim::PWMSim rightLeader {1};
// Simulation Physics
// Configure these to match your drivetrain's physical dimensions
// and characterization results.
decltype(1_V / 1_mps) kv = 1.98 * 1_V * 1_s / 1_m;
decltype(1_V / 1_mps_sq) ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
decltype(1_V / 1_rad_per_s) kvAngular = 1.5 * 1_V * 1_s / 1_rad;
decltype(1_V / 1_rad_per_s_sq) kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad;
const frc::LinearSystem<2, 2, 2> kDrivetrainPlant =
frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, kaAngular);
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
kDrivetrainPlant,
2.0_ft,
frc::DCMotor::CIM(2),
8.0,
6.0_in / 2,
{0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
// Simulated Vision System.
// Configure these to match your PhotonVision Camera,
// pipeline, and LED setup.
units::degree_t camDiagFOV = 170.0_deg; // assume wide-angle camera
units::degree_t camPitch = 15_deg;
units::meter_t camHeightOffGround = 24_in;
units::meter_t maxLEDRange = 20_m;
int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels
double minTargetArea = 10; // square pixels
photonlib::SimVisionSystem simVision {
"photonvision",
camDiagFOV,
camPitch,
frc::Transform2d{},
camHeightOffGround,
maxLEDRange,
camResolutionWidth,
camResolutionHeight,
minTargetArea};
// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf page 208
const units::meter_t targetWidth = 41.30_in - 6.70_in;
// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf page 197
const units::meter_t targetHeight = 98.19_in - 81.19_in; // meters
// See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf pages 4 and 5
const units::meter_t tgtXPos = 54_ft;
const units::meter_t tgtYPos = (27.0_ft / 2) - 43.75_in - (48.0_in / 2.0);
const frc::Translation2d targetTrans {tgtXPos, tgtYPos};
const frc::Rotation2d targetRot {0.0_deg};
frc::Pose2d farTargetPose {targetTrans, targetRot};
frc::Field2d field {};
};
/*
* 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 <https://www.gnu.org/licenses/>.
*/
#include <photonlib/SimVisionSystem.h>
#include <frc/DriverStation.h>
#include <frc/RobotController.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Translation2d.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/PWMSim.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/time.h>
#pragma once
class DrivetrainSim {
public:
DrivetrainSim() {
simVision.AddSimVisionTarget(photonlib::SimVisionTarget(
farTargetPose, 81.91_in, targetWidth, targetHeight));
frc::SmartDashboard::PutData("Field", &field);
}
void update();
private:
// Simulated Motor Controllers
frc::sim::PWMSim leftLeader{0};
frc::sim::PWMSim rightLeader{1};
// Simulation Physics
// Configure these to match your drivetrain's physical dimensions
// and characterization results.
static constexpr decltype(1_V / 1_mps) kv = 1.98 * 1_V * 1_s / 1_m;
static constexpr decltype(1_V / 1_mps_sq) ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
static constexpr decltype(1_V / 1_rad_per_s) kvAngular =
1.5 * 1_V * 1_s / 1_rad;
static constexpr decltype(1_V / 1_rad_per_s_sq) kaAngular =
0.3 * 1_V * 1_s * 1_s / 1_rad;
static constexpr units::meter_t kTrackWidth = 1_m;
const frc::LinearSystem<2, 2, 2> kDrivetrainPlant =
frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular,
kaAngular, kTrackWidth);
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
kDrivetrainPlant, 2.0_ft,
frc::DCMotor::CIM(2), 8.0,
6.0_in / 2, {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
// Simulated Vision System.
// Configure these to match your PhotonVision Camera,
// pipeline, and LED setup.
units::degree_t camDiagFOV = 170.0_deg; // assume wide-angle camera
units::degree_t camPitch = 15_deg;
units::meter_t camHeightOffGround = 24_in;
units::meter_t maxLEDRange = 20_m;
int camResolutionWidth = 640; // pixels
int camResolutionHeight = 480; // pixels
double minTargetArea = 10; // square pixels
photonlib::SimVisionSystem simVision{
"photonvision", camDiagFOV, camPitch,
frc::Transform2d{}, camHeightOffGround, maxLEDRange,
camResolutionWidth, camResolutionHeight, minTargetArea};
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 208
const units::meter_t targetWidth = 41.30_in - 6.70_in;
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
// page 197
const units::meter_t targetHeight = 98.19_in - 81.19_in; // meters
// See
// https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
// pages 4 and 5
const units::meter_t tgtXPos = 54_ft;
const units::meter_t tgtYPos = (27.0_ft / 2) - 43.75_in - (48.0_in / 2.0);
const frc::Translation2d targetTrans{tgtXPos, tgtYPos};
const frc::Rotation2d targetRot{0.0_deg};
frc::Pose2d farTargetPose{targetTrans, targetRot};
frc::Field2d field{};
};

14
photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h
View File

@@ -1,5 +1,5 @@
/**
* Copyright (C) 2018-2020 Photon Vision.
/*
* 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
@@ -17,24 +17,24 @@
#pragma once
#include "DrivetrainSim.h"
#include <photonlib/PhotonCamera.h>
#include <frc/PWMVictorSPX.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMVictorSPX.h>
#include <units/angle.h>
#include <units/length.h>
#include "DrivetrainSim.h"
class Robot : public frc::TimedRobot {
public:
public:
void TeleopPeriodic() override;
void SimulationPeriodic() override;
private:
private:
// Constants such as camera and target height stored. Change per robot and
// goal!
const units::meter_t CAMERA_HEIGHT = 24_in;