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

Sim Updates for 2023 (#512)

Browse Source 
* WIP updating sim stuff for 2023 and pose3d's

* vision system build fixups, but test not yet passing.

* WIP Sim fixups and working on testcases

* Still doesn't work, but closer

* tests pass

* removed C++ sim support

* formatting update

* adjusted target height above ground per review

* Turns out its unused

* missed example removal
This commit is contained in:
Chris Gerth
2022-11-03 15:05:37 -05:00
committed by GitHub
parent a64697e714
commit b408a58e9e
18 changed files with 276 additions and 1314 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
photonlib-cpp-examples/src/main/cpp/examples/examples.json
View File

@@ -35,16 +35,4 @@
"dependencies": [],
"foldername": "aimandrange"
}
{
"name": "SimAimAndRange",
"description": "Adding Simulation Support to the Aim And Range example",
"tags": [],
"gradlebase": "cpp",
"language": "cpp",
"commandversion": 1,
"mainclass": "Main",
"packagetoreplace": null,
"dependencies": [],
"foldername": "simaimandrange"
}
]

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

@@ -1,73 +0,0 @@
/*
* 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.
*/
#include "Robot.h"
#include <photonlib/PhotonUtils.h>
void Robot::TeleopPeriodic() {
double forwardSpeed;
double rotationSpeed;
if (xboxController.GetAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
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()});
// 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.value(),
GOAL_RANGE_METERS.value());
// 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);
}
void Robot::SimulationPeriodic() { dtSim.update(); }
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

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

@@ -1,50 +0,0 @@
/*
* 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.
*/
#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());
}

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

@@ -1,111 +0,0 @@
/*
* 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.
*/
#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::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,
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{};
};

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

@@ -1,77 +0,0 @@
/*
* 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.
*/
#pragma once
#include <photonlib/PhotonCamera.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:
void TeleopPeriodic() override;
void SimulationPeriodic() override;
private:
// Constants such as camera and target height stored. Change per robot and
// goal!
const units::meter_t CAMERA_HEIGHT = 24_in;
const units::meter_t TARGET_HEIGHT = 81.19_in;
// Angle between horizontal and the camera.
const units::radian_t CAMERA_PITCH = 15_deg;
// How far from the target we want to be
const units::meter_t GOAL_RANGE_METERS = 10_ft;
// PID constants should be tuned per robot
const double LINEAR_P = 2.0;
const double LINEAR_D = 0.0;
frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D};
const double ANGULAR_P = 0.03;
const double ANGULAR_D = 0.003;
frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D};
// Change this to match the name of your camera
photonlib::PhotonCamera camera{"photonvision"};
frc::XboxController xboxController{0};
// Drive motors
frc::PWMVictorSPX leftMotor{0};
frc::PWMVictorSPX rightMotor{1};
frc::DifferentialDrive drive{leftMotor, rightMotor};
// Simulation Support
DrivetrainSim dtSim{};
};