Merge branch 'master' into enable-aruco

photon-client/src/components/app/photon-3d-visualizer.vue

@@ -4,18 +4,18 @@ import { onBeforeUnmount, onMounted, watchEffect } from "vue";
 import {
   ArrowHelper,
   BoxGeometry,
+  Color,
   ConeGeometry,
   Mesh,
   MeshNormalMaterial,
+  type Object3D,
   PerspectiveCamera,
   Quaternion,
   Scene,
   Vector3,
-  Color,
   WebGLRenderer
 } from "three";
 import { TrackballControls } from "three/examples/jsm/controls/TrackballControls";
-import { type Object3D } from "three";
 
 const props = defineProps<{
   targets: PhotonTarget[];

photon-client/src/components/app/photon-log-view.vue

@@ -1,5 +1,5 @@
 <script setup lang="ts">
-import { computed, ref, inject } from "vue";
+import { computed, inject, ref } from "vue";
 import { LogLevel, type LogMessage } from "@/types/SettingTypes";
 import { useStateStore } from "@/stores/StateStore";
 

photon-client/src/components/cameras/CameraCalibrationCard.vue

@@ -1,7 +1,7 @@
 <script setup lang="ts">
 import { computed, ref } from "vue";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
-import { CalibrationBoardTypes, type VideoFormat, type Resolution } from "@/types/SettingTypes";
+import { CalibrationBoardTypes, type Resolution, type VideoFormat } from "@/types/SettingTypes";
 import JsPDF from "jspdf";
 import { font as PromptRegular } from "@/assets/fonts/PromptRegular";
 import MonoLogo from "@/assets/images/logoMono.png";
@@ -150,7 +150,10 @@ const importCalibrationFromCalibDB = ref();
 const openCalibUploadPrompt = () => {
   importCalibrationFromCalibDB.value.click();
 };
-const readImportedCalibration = ({ files }: { files: FileList }) => {
+const readImportedCalibration = (payload: Event) => {
+  if (payload.target == null || !payload.target?.files) return;
+  const files: FileList = payload.target.files as FileList;
+
   files[0].text().then((text) => {
     useCameraSettingsStore()
       .importCalibDB({ payload: text, filename: files[0].name })

photon-client/src/components/settings/DeviceControlCard.vue

@@ -63,14 +63,17 @@ const offlineUpdate = ref();
 const openOfflineUpdatePrompt = () => {
   offlineUpdate.value.click();
 };
-const handleOfflineUpdate = ({ files }: { files: FileList }) => {
+const handleOfflineUpdate = (payload: Event) => {
   useStateStore().showSnackbarMessage({
     message: "New Software Upload in Progress...",
     color: "secondary",
     timeout: -1
   });
 
   const formData = new FormData();
+
+  if (payload.target == null || !payload.target?.files) return;
+  const files: FileList = payload.target.files as FileList;
   formData.append("jarData", files[0]);
 
   axios

photon-client/src/stores/settings/CameraSettingsStore.ts

@@ -11,8 +11,7 @@ import { PlaceholderCameraSettings } from "@/types/SettingTypes";
 import { useStateStore } from "@/stores/StateStore";
 import type { WebsocketCameraSettingsUpdate } from "@/types/WebsocketDataTypes";
 import { WebsocketPipelineType } from "@/types/WebsocketDataTypes";
-import type { ActiveConfigurablePipelineSettings, ActivePipelineSettings } from "@/types/PipelineTypes";
-import type { PipelineType } from "@/types/PipelineTypes";
+import type { ActiveConfigurablePipelineSettings, ActivePipelineSettings, PipelineType } from "@/types/PipelineTypes";
 import axios from "axios";
 
 interface CameraSettingsStore {
@@ -174,9 +173,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
       settings: Partial<ActivePipelineSettings>,
       cameraIndex: number = useStateStore().currentCameraIndex
     ) {
-      Object.entries(settings).forEach(([k, v]) => {
-        this.cameras[cameraIndex].pipelineSettings[k] = v;
-      });
+      Object.assign(this.cameras[cameraIndex].pipelineSettings, settings);
     },
     /**
      * Change the nickname of the currently selected pipeline of the provided camera.

photon-client/src/stores/settings/GeneralSettingsStore.ts

@@ -1,10 +1,10 @@
 import { defineStore } from "pinia";
 import type {
+  ConfigurableNetworkSettings,
   GeneralSettings,
   LightingSettings,
   MetricData,
-  NetworkSettings,
-  ConfigurableNetworkSettings
+  NetworkSettings
 } from "@/types/SettingTypes";
 import { NetworkConnectionType } from "@/types/SettingTypes";
 import { useStateStore } from "@/stores/StateStore";

photon-client/src/types/WebsocketDataTypes.ts

@@ -1,6 +1,5 @@
-import type { GeneralSettings, LightingSettings, MetricData, NetworkSettings } from "@/types/SettingTypes";
+import type { GeneralSettings, LightingSettings, LogLevel, MetricData, NetworkSettings } from "@/types/SettingTypes";
 import type { ActivePipelineSettings } from "@/types/PipelineTypes";
-import type { LogLevel } from "@/types/SettingTypes";
 
 export interface WebsocketLogMessage {
   logMessage: {

photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Constants.java

@@ -0,0 +1,91 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 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.
+ */
+
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+    // ---------- Vision
+    // Constants about how your camera is mounted to the robot
+    public static final double CAMERA_PITCH_RADIANS =
+            Units.degreesToRadians(15); // Angle "up" from horizontal
+    public static final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24); // Height above floor
+
+    // How far from the target we want to be
+    public static final double GOAL_RANGE_METERS = Units.feetToMeters(10);
+
+    // Where the 2020 High goal target is located on the field
+    // See
+    // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system
+    // and https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
+    // (pages 4 and 5)
+    public static final Pose3d TARGET_POSE =
+            new Pose3d(
+                    new Translation3d(
+                            Units.feetToMeters(52.46),
+                            Units.inchesToMeters(94.66),
+                            Units.inchesToMeters(89.69)), // (center of vision target)
+                    new Rotation3d(0.0, 0.0, Math.PI));
+    // ----------
+
+    // ---------- Drivetrain
+    public static final int LEFT_MOTOR_CHANNEL = 0;
+    public static final int RIGHT_MOTOR_CHANNEL = 1;
+
+    // PID constants should be tuned per robot
+    public static final double LINEAR_P = 0.5;
+    public static final double LINEAR_I = 0;
+    public static final double LINEAR_D = 0.1;
+
+    public static final double ANGULAR_P = 0.03;
+    public static final double ANGULAR_I = 0;
+    public static final double ANGULAR_D = 0.003;
+
+    // Ratio to multiply joystick inputs by
+    public static final double DRIVESPEED = 0.75;
+
+    // The following properties are necessary for simulation:
+
+    // Distance from drivetrain left wheels to right wheels
+    public static final double TRACKWIDTH_METERS = Units.feetToMeters(2.0);
+    public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(6.0);
+
+    // The motors used in the gearbox for one drivetrain side
+    public static final DCMotor DRIVE_MOTORS = DCMotor.getCIM(2);
+    // The gearbox reduction, or how many motor rotations per wheel rotation
+    public static final double GEARING = 8.0;
+
+    // The drivetrain feedforward values
+    public static final double LINEAR_KV = 2.0;
+    public static final double LINEAR_KA = 0.5;
+
+    public static final double ANGULAR_KV = 2.25;
+    public static final double ANGULAR_KA = 0.3;
+    // ----------
+}

photonlib-java-examples/simaimandrange/src/main/java/frc/robot/Robot.java

@@ -24,13 +24,16 @@
 
 package frc.robot;
 
+import static frc.robot.Constants.*;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
 import frc.robot.sim.DrivetrainSim;
+import frc.robot.sim.VisionSim;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonUtils;
 
@@ -41,34 +44,18 @@
  * project.
  */
 public class Robot extends TimedRobot {
-    // 2020 High goal target height above ground
-    public static final double TARGET_HEIGHT_METERS = Units.inchesToMeters(81.19);
-
-    // Constants about how your camera is mounted to the robot
-    public static final double CAMERA_PITCH_RADIANS =
-            Units.degreesToRadians(15); // Angle "up" from horizontal
-    public static 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(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.5;
-    final double LINEAR_D = 0.1;
     PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
-
-    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);
 
     // Drive motors
-    PWMVictorSPX leftMotor = new PWMVictorSPX(0);
-    PWMVictorSPX rightMotor = new PWMVictorSPX(1);
+    PWMVictorSPX leftMotor = new PWMVictorSPX(LEFT_MOTOR_CHANNEL);
+    PWMVictorSPX rightMotor = new PWMVictorSPX(RIGHT_MOTOR_CHANNEL);
     DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
 
     @Override
@@ -92,7 +79,7 @@ public void teleopPeriodic() {
                 double range =
                         PhotonUtils.calculateDistanceToTargetMeters(
                                 CAMERA_HEIGHT_METERS,
-                                TARGET_HEIGHT_METERS,
+                                TARGET_POSE.getZ(),
                                 CAMERA_PITCH_RADIANS,
                                 Units.degreesToRadians(result.getBestTarget().getPitch()));
 
@@ -110,8 +97,8 @@ public void teleopPeriodic() {
             }
         } else {
             // Manual Driver Mode
-            forwardSpeed = -xboxController.getLeftY() * 0.75;
-            rotationSpeed = -xboxController.getRightX() * 0.75;
+            forwardSpeed = -xboxController.getLeftY() * DRIVESPEED;
+            rotationSpeed = -xboxController.getRightX() * DRIVESPEED;
         }
 
         // Use our forward/turn speeds to control the drivetrain
@@ -122,14 +109,17 @@ public void teleopPeriodic() {
     // Simulation support
 
     DrivetrainSim dtSim;
+    VisionSim visionSim;
 
     @Override
     public void simulationInit() {
-        dtSim = new DrivetrainSim(leftMotor.getChannel(), rightMotor.getChannel(), camera);
+        dtSim = new DrivetrainSim(leftMotor, rightMotor);
+        visionSim = new VisionSim("main", camera);
     }
 
     @Override
     public void simulationPeriodic() {
         dtSim.update();
+        visionSim.update(dtSim.getPose());
     }
 }