Added sim support to aim and range

java/aim-and-range/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2020",
+    "projectYear": "2021",
     "teamNumber": 1
 }

java/aim-and-range/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.3.2"
+    id "edu.wpi.first.GradleRIO" version "2021.1.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

java/aim-and-range/src/main/java/frc/robot/Robot.java

@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.util.Units;
+import frc.robot.sim.DrivetrainSim;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -36,25 +37,27 @@
  * project.
  */
 public class Robot extends TimedRobot {
-   // Constants such as camera and target height stored. Change per robot and goal!
-   final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
-   final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);
-   // Angle between horizontal and the camera.
-   final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);
+
+   // 2020 High goal target height above ground
+   static public final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
+
+   // Constants about how your camera is mounted to the robot
+   static public final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(15); // Angle "up" from horizontal
+   static public final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);   // Height above floor
 
    // How far from the target we want to be
-   final double GOAL_RANGE_METERS = Units.feetToMeters(3);
+   final double GOAL_RANGE_METERS = Units.feetToMeters(10);
 
    // Change this to match the name of your camera
    PhotonCamera camera = new PhotonCamera("photonvision");
 
    // PID constants should be tuned per robot
-   final double LINEAR_P = 0.1;
+   final double LINEAR_P = 2.0;
    final double LINEAR_D = 0.0;
    PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
 
-   final double ANGULAR_P = 0.1;
-   final double ANGULAR_D = 0.0;
+   final double ANGULAR_P = 0.03;
+   final double ANGULAR_D = 0.003;
    PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
 
    XboxController xboxController = new XboxController(0);
@@ -77,25 +80,43 @@ public void teleopPeriodic() {
          if (result.hasTargets()) {
             // First calculate range
             double range = PhotonUtils.calculateDistanceToTargetMeters(CAMERA_HEIGHT_METERS, TARGET_HEIGHT_METERS,
-                  CAMERA_PITCH_RADIANS, result.getBestTarget().getPitch());
+                  CAMERA_PITCH_RADIANS, Units.degreesToRadians(result.getBestTarget().getPitch()));
 
             // Use this range as the measurement we give to the PID controller.
-            forwardSpeed = forwardController.calculate(range, GOAL_RANGE_METERS);
+            // -1.0 required to ensure positive PID controller effort _increases_ range
+            forwardSpeed = -1.0 * forwardController.calculate(range, GOAL_RANGE_METERS);
 
             // 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;
             rotationSpeed = 0;
          }
       } else {
          // Manual Driver Mode
-         forwardSpeed = xboxController.getY(GenericHID.Hand.kRight);
-         rotationSpeed = xboxController.getX(GenericHID.Hand.kLeft);
+         forwardSpeed  = -1.0* xboxController.getY(GenericHID.Hand.kLeft);
+         rotationSpeed = xboxController.getX(GenericHID.Hand.kRight);
       }
 
       // Use our forward/turn speeds to control the drivetrain
       drive.arcadeDrive(forwardSpeed, rotationSpeed);
    }
+
+
+   ////////////////////////////////////////////////////
+   // Simulation support
+
+   DrivetrainSim dtSim;
+
+   @Override
+   public void simulationInit(){
+      dtSim = new DrivetrainSim();
+   }
+
+   @Override
+   public void simulationPeriodic(){
+      dtSim.update();
+   }
 }

java/aim-and-range/src/main/java/frc/robot/sim/DrivetrainSim.java

@@ -0,0 +1,112 @@
+package frc.robot.sim;
+
+import org.photonvision.SimVisionSystem;
+import org.photonvision.SimVisionTarget;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.system.LinearSystem;
+import edu.wpi.first.wpilibj.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+
+/**
+ * Implementation of a simulation of robot physics, sensors, motor controllers
+ * Includes a Simulated PhotonVision system and one vision target.
+ * 
+ * This class and its methods are only relevant during simulation. While on the
+ * real robot, the real motors/sensors/physics are used instead.
+ */
+public class DrivetrainSim {
+
+    // Simulated Motor Controllers
+    PWMSim leftLeader = new PWMSim(0);
+    PWMSim rightLeader = new PWMSim(1);
+
+    // Simulation Physics
+    // Configure these to match your drivetrain's physical dimensions
+    // and characterization results.
+    LinearSystem<N2, N2, N2> drivetrainSystem = LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+    DifferentialDrivetrainSim drivetrainSimulator = new DifferentialDrivetrainSim(drivetrainSystem, DCMotor.getCIM(2),
+            8, Units.feetToMeters(2.0), Units.inchesToMeters(6.0/2.0), null);
+
+    // Simulated Vision System.
+    // Configure these to match your PhotonVision Camera,
+    // pipeline, and LED setup.
+    double camDiagFOV = 170.0; // degrees - assume wide-angle camera
+    double camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees
+    double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters
+    double maxLEDRange = 20; // meters
+    int camResolutionWidth = 640; // pixels
+    int camResolutionHeight = 480; // pixels
+    double minTargetArea = 10; // square pixels
+
+    SimVisionSystem simVision = new SimVisionSystem("photonvision", camDiagFOV, camPitch, new Transform2d(),
+            camHeightOffGround, maxLEDRange, camResolutionWidth, camResolutionHeight, minTargetArea);
+
+    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf page 208
+    double targetWidth = Units.inchesToMeters(41.30) - Units.inchesToMeters(6.70); // meters
+    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf page 197
+    double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters
+    // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf pages 4 and 5
+    double tgtXPos = Units.feetToMeters(54);
+    double tgtYPos = Units.feetToMeters(27/2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0/2.0) ;
+    Pose2d farTargetPose = new Pose2d(new Translation2d(tgtXPos, tgtYPos), new Rotation2d(0.0));
+
+
+    Field2d field = new Field2d();
+
+    public DrivetrainSim() {
+        simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, Robot.TARGET_HEIGHT_METERS, targetWidth, targetHeight));
+        SmartDashboard.putData("Field", field);
+    }
+
+    /**
+     * Perform all periodic drivetrain simulation related tasks to advance our
+     * simulation of robot physics forward by a single 20ms step.
+     */
+    public void update() {
+
+        double leftMotorCmd = 0;
+        double rightMotorCmd = 0;
+
+        if (DriverStation.getInstance().isEnabled() && !RobotController.isBrownedOut()) {
+            leftMotorCmd = leftLeader.getSpeed();
+            rightMotorCmd = rightLeader.getSpeed();
+        }
+
+        drivetrainSimulator.setInputs(leftMotorCmd * RobotController.getInputVoltage(),
+                -rightMotorCmd * RobotController.getInputVoltage());
+        drivetrainSimulator.update(0.02);
+
+        // Update PhotonVision based on our new robot position.
+        simVision.processFrame(drivetrainSimulator.getPose());
+
+        field.setRobotPose(drivetrainSimulator.getPose());
+    }
+
+    /**
+     * Resets the simulation back to a pre-defined pose Useful to simulate the
+     * action of placing the robot onto a specific spot in the field (IE, at the
+     * start of each match).
+     * 
+     * @param pose
+     */
+    public void resetPose(Pose2d pose) {
+        drivetrainSimulator.setPose(pose);
+    }
+
+}

java/aim-and-range/vendordeps/photonlib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "2020.0.11",
+    "version": "2021.1.2",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
     "mavenUrls": [
         "https://maven.photonvision.org/repository/internal"
@@ -12,7 +12,7 @@
         {
             "groupId": "org.photonvision.lib",
             "artifactId": "PhotonLib-cpp",
-            "version": "2020.0.11",
+            "version": "2021.1.2",
             "libName": "Photon",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -29,7 +29,7 @@
         {
             "groupId": "org.photonvision.lib",
             "artifactId": "PhotonLib-java",
-            "version": "2020.0.11"
+            "version": "2021.1.2"
         }
     ]
-}
+}