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>

photonlib-cpp-examples/src/main/cpp/examples/aimandrange/cpp/Robot.cpp

@@ -36,12 +36,14 @@ void Robot::TeleopPeriodic() {
               units::degree_t{result.GetBestTarget().GetPitch()});
 
       // Use this range as the measurement we give to the PID controller.
-      forwardSpeed = forwardController.Calculate(
+      // -1.0 required to ensure positive PID controller effort _increases_ range
+      forwardSpeed = -1.0* forwardController.Calculate(
           range.to<double>(), GOAL_RANGE_METERS.to<double>());
 
       // Also calculate angular power
-      rotationSpeed =
-          turnController.Calculate(result.GetBestTarget().GetYaw(), 0);
+      // -1.0 required to ensure positive PID controller effort _increases_ yaw
+      rotationSpeed = -1.0 * turnController.Calculate(
+          result.GetBestTarget().GetYaw(), 0);
     } else {
       // If we have no targets, stay still.
       forwardSpeed = 0;
@@ -50,7 +52,7 @@ void Robot::TeleopPeriodic() {
   } else {
     // Manual Driver Mode
     forwardSpeed =
-        xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
+        -1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
     rotationSpeed =
         xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
   }

photonlib-cpp-examples/src/main/cpp/examples/aimattarget/cpp/Robot.cpp

@@ -21,7 +21,7 @@
 
 void Robot::TeleopPeriodic() {
   double forwardSpeed =
-      xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
+      -1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
   double rotationSpeed;
 
   if (xboxController.GetAButton()) {
@@ -31,7 +31,7 @@ void Robot::TeleopPeriodic() {
 
     if (result.HasTargets()) {
       // Rotation speed is the output of the PID controller
-      rotationSpeed = controller.Calculate(result.GetBestTarget().GetYaw(), 0);
+      rotationSpeed = -1.0 * controller.Calculate(result.GetBestTarget().GetYaw(), 0);
     } else {
       // If we have no targets, stay still.
       rotationSpeed = 0;

photonlib-cpp-examples/src/main/cpp/examples/examples.json

@@ -35,4 +35,16 @@
     "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"
+  }
 ]

photonlib-cpp-examples/src/main/cpp/examples/getinrange/cpp/Robot.cpp

@@ -36,7 +36,7 @@ void Robot::TeleopPeriodic() {
           units::degree_t{result.GetBestTarget().GetPitch()});
 
       // Use this range as the measurement we give to the PID controller.
-      forwardSpeed = controller.Calculate(range.to<double>(),
+      forwardSpeed = -1.0 * controller.Calculate(range.to<double>(),
                                           GOAL_RANGE_METERS.to<double>());
     } else {
       // If we have no targets, stay still.
@@ -45,7 +45,7 @@ void Robot::TeleopPeriodic() {
   } else {
     // Manual Driver Mode
     forwardSpeed =
-        xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
+        -1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
   }
 
   // Use our forward/turn speeds to control the drivetrain

photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp

@@ -0,0 +1,72 @@
+/**
+ * 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 "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 = -1.0* forwardController.Calculate(
+          range.to<double>(), GOAL_RANGE_METERS.to<double>());
+
+      // Also calculate angular power
+      // -1.0 required to ensure positive PID controller effort _increases_ yaw
+      rotationSpeed = -1.0 * turnController.Calculate(
+          result.GetBestTarget().GetYaw(), 0);
+    } else {
+      // If we have no targets, stay still.
+      forwardSpeed = 0;
+      rotationSpeed = 0;
+    }
+  } else {
+    // Manual Driver Mode
+    forwardSpeed =
+        -1.0 * xboxController.GetY(frc::GenericHID::JoystickHand::kRightHand);
+    rotationSpeed =
+        xboxController.GetX(frc::GenericHID::JoystickHand::kLeftHand);
+  }
+
+  // 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
+
+

photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp

@@ -0,0 +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());
+}

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 {};
+
+};

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

@@ -0,0 +1,70 @@
+/**
+ * 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/>.
+ */
+
+#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 <units/angle.h>
+#include <units/length.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{};
+};