Add Basic Sim Example (#237)

* WIP adding sim pose example * WIP making examples buildable like WPI. Not quite there yet.... * Make examples runnable * remove lock * add lock * WIP Adding a simpler example for simulation * Spotless Apply * Added simulation-supporting aim and range example * Spotless, revised hand usage to be consistent across examples, and propagated required -1.0's to non-sim examples. Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
2026-06-20 00:51:41 +00:00 · 2021-01-26 22:26:15 -06:00
parent 4c15a46cda
commit 0330467874
19 changed files with 708 additions and 29 deletions
--- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h
+++ b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h
@@ -0,0 +1,107 @@
+/**
+ * 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 {};
+
+};