mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
a764ace7f2
(Very) beta AprilTag support in PhotonVision. Disables Picam GPU acceleration until we can debug auto exposure in the MMAL driver. Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com> Co-authored-by: Matt <matthew.morley.ca@gmail.com> Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com> Co-authored-by: Chris <chrisgerth010592@gmail.com> Co-authored-by: mdurrani808 <mdurrani808@gmail.com>
112 lines
4.4 KiB
C++
112 lines
4.4 KiB
C++
/*
|
|
* 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{};
|
|
};
|