conditional sim

frc2053 · Sep 12, 2024 · 980210a · 980210a
1 parent 6a1814f
commit 980210a
Show file tree

Hide file tree

Showing 5 changed files with 201 additions and 25 deletions.
diff --git a/aslayout.json b/aslayout.json
@@ -1,21 +1,41 @@
 {
   "hubs": [
     {
-      "x": 345,
-      "y": 164,
-      "width": 1363,
-      "height": 791,
+      "x": -2568,
+      "y": -190,
+      "width": 2576,
+      "height": 1408,
       "state": {
         "sidebar": {
-          "width": 300,
+          "width": 410,
           "expanded": [
-            "/SwerveDrive/PoseEstimatorPose",
             "/SwerveDrive/PoseEstimatorPose/rotation",
-            "/PathPlanner"
+            "/SwerveDrive/CurrentStates/0",
+            "/SwerveDrive/DesiredStates/0",
+            "/SwerveDrive/CurrentPositions/0",
+            "/SwerveDrive/CurrentPositions/0/angle",
+            "/SwerveDrive/CurrentPositions/1",
+            "/SwerveDrive/CurrentPositions/1/angle",
+            "/SwerveDrive/CurrentPositions/2",
+            "/SwerveDrive/CurrentPositions/2/angle",
+            "/SwerveDrive/CurrentPositions/3",
+            "/SwerveDrive/CurrentPositions/3/angle",
+            "/SwerveDrive/DesiredStates/0/angle",
+            "/SwerveDrive/CurrentStates/0/angle",
+            "/photonvision/fl_cam",
+            "/SwerveDrive/PoseEstimatorPose",
+            "/Vision",
+            "/Vision/fl_camPoseEstimation",
+            "/Vision/fl_camPoseEstimation/translation",
+            "/Vision/fl_camPoseEstimation/rotation",
+            "/NoteVisualizer/LaunchedNotes",
+            "/SwerveDrive",
+            "/SwerveDrive/CurrentStates",
+            "/SwerveDrive/DesiredStates"
           ]
         },
         "tabs": {
-          "selected": 2,
+          "selected": 5,
           "tabs": [
             {
               "type": 0,
@@ -31,7 +51,18 @@
                     "type": null,
                     "factor": 1
                   },
-                  "fields": []
+                  "fields": [
+                    {
+                      "key": "NT:/SwerveDrive/CurrentStates/0/speed",
+                      "color": "#2b66a2",
+                      "show": true
+                    },
+                    {
+                      "key": "NT:/SwerveDrive/DesiredStates/0/speed",
+                      "color": "#e5b31b",
+                      "show": true
+                    }
+                  ]
                 },
                 "discrete": {
                   "fields": []
@@ -42,17 +73,65 @@
                     "type": null,
                     "factor": 1
                   },
-                  "fields": []
+                  "fields": [
+                    {
+                      "key": "NT:/SwerveDrive/CurrentStates/0/angle/value",
+                      "color": "#af2437",
+                      "show": true
+                    },
+                    {
+                      "key": "NT:/SwerveDrive/DesiredStates/0/angle/value",
+                      "color": "#80588e",
+                      "show": true
+                    }
+                  ]
                 }
               },
               "title": "Line Graph"
             },
+            {
+              "type": 4,
+              "fields": [
+                null,
+                null,
+                null
+              ],
+              "selectionType": "full",
+              "selectionRangeMin": 0,
+              "selectionRangeMax": 10,
+              "measurementType": "independent",
+              "measurementSampling": "fixed",
+              "measurementSamplingPeriod": 20,
+              "histogramMin": 0,
+              "histogramMax": 10,
+              "histogramStep": 1,
+              "title": "Statistics"
+            },
             {
               "type": 6,
               "uuid": "7azj8gpne5ee1gxp8gnulcpeg1hzrw9d",
               "fields": [],
               "listFields": [
-                [],
+                [
+                  {
+                    "type": "Game Piece 0",
+                    "key": "NT:/NoteVisualizer/StagedNotes",
+                    "sourceTypeIndex": 2,
+                    "sourceType": "Pose3d[]"
+                  },
+                  {
+                    "type": "Game Piece 0",
+                    "key": "NT:/NoteVisualizer/LaunchedNotes",
+                    "sourceTypeIndex": 2,
+                    "sourceType": "Pose3d[]"
+                  },
+                  {
+                    "type": "Game Piece 0",
+                    "key": "NT:/NoteVisualizer/RobotNote",
+                    "sourceTypeIndex": 1,
+                    "sourceType": "Pose3d"
+                  }
+                ],
                 [
                   {
                     "type": "Robot",
@@ -95,33 +174,79 @@
                     "key": "NT:/PathPlanner/activePath",
                     "sourceTypeIndex": 0,
                     "sourceType": 5
+                  },
+                  {
+                    "type": "Trajectory",
+                    "key": "NT:/SwerveDrive/CurrentChoreoTrajectory",
+                    "sourceTypeIndex": 2,
+                    "sourceType": "Pose2d[]"
                   }
                 ]
               ],
               "options": {
                 "field": "2024 Field",
                 "alliance": "blue",
-                "robot": "20532024",
+                "robot": "LynkClone",
                 "unitDistance": "meters",
                 "unitRotation": "radians"
               },
               "configHidden": false,
               "visualizer": {
-                "cameraIndex": -1,
-                "orbitFov": 50,
+                "cameraIndex": -2,
+                "orbitFov": 75,
                 "cameraPosition": [
-                  -3.5853041954079066,
-                  3.1117141052284785,
-                  6.420683499878898
+                  0.0948801836520725,
+                  3.2238861086067025,
+                  0.2469499751943985
                 ],
                 "cameraTarget": [
-                  -4.27671200193653,
-                  0.19080418080003858,
-                  2.121466287955874
+                  0.094882990675214,
+                  0.41078572113049105,
+                  0.24694978849037907
                 ]
               },
               "title": "3D Field"
             },
+            {
+              "type": 5,
+              "uuid": "surohaj5wfsq7ktr2nxzab22oc0ynqe9",
+              "fields": [],
+              "listFields": [
+                [
+                  {
+                    "type": "Trajectory",
+                    "key": "NT:/SwerveDrive/CurrentChoreoTrajectory",
+                    "sourceTypeIndex": 2,
+                    "sourceType": "Pose2d[]"
+                  },
+                  {
+                    "type": "Robot",
+                    "key": "NT:/SwerveDrive/PoseEstimatorPose",
+                    "sourceTypeIndex": 1,
+                    "sourceType": "Pose2d"
+                  },
+                  {
+                    "type": "Ghost",
+                    "key": "NT:/Vision/fl_camPoseEstimation",
+                    "sourceTypeIndex": 1,
+                    "sourceType": "Pose2d"
+                  }
+                ]
+              ],
+              "options": {
+                "game": "2024 Field",
+                "unitDistance": "meters",
+                "unitRotation": "radians",
+                "origin": "right",
+                "size": 0.868426,
+                "allianceBumpers": "auto",
+                "allianceOrigin": "blue",
+                "orientation": "blue left, red right"
+              },
+              "configHidden": false,
+              "visualizer": null,
+              "title": "Odometry"
+            },
             {
               "type": 9,
               "uuid": "ldza8g2yvu4juu4c201rjah0f42x3bq4",
@@ -144,16 +269,42 @@
               ],
               "listFields": [],
               "options": {
-                "maxSpeed": 5.7,
+                "maxSpeed": 6.037576989785339,
                 "rotationUnits": "radians",
                 "arrangement": "0,1,2,3",
-                "sizeLeftRight": 0.79375,
-                "sizeFrontBack": 0.79375,
+                "sizeLeftRight": 0.55245,
+                "sizeFrontBack": 0.40005,
                 "forwardDirection": "up"
               },
               "configHidden": false,
               "visualizer": null,
               "title": "Swerve"
+            },
+            {
+              "type": 1,
+              "legendHeight": 0.3,
+              "legends": {
+                "left": {
+                  "lockedRange": null,
+                  "unitConversion": {
+                    "type": null,
+                    "factor": 1
+                  },
+                  "fields": []
+                },
+                "discrete": {
+                  "fields": []
+                },
+                "right": {
+                  "lockedRange": null,
+                  "unitConversion": {
+                    "type": null,
+                    "factor": 1
+                  },
+                  "fields": []
+                }
+              },
+              "title": "Line Graph"
             }
           ]
         }

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -11,8 +11,11 @@
 #include "frc/Alert.h"
 
 #include "str/DataUtils.h"
+#include "frc/Threads.h"
 
 void Robot::RobotInit() {
+  //DANGEROUS MAKE SURE CODE DOESN'T BLOCK!!!
+  frc::SetCurrentThreadPriority(true, 15);
   str::DataUtils::LogGitInfo();
   frc2::CommandScheduler::GetInstance().SetPeriod(consts::LOOP_PERIOD);
   frc::DataLogManager::Start();

diff --git a/src/main/deploy/branch.txt b/src/main/deploy/branch.txt
@@ -1 +1 @@
-shooter
+main
diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-a411e2f
+6a1814f
diff --git a/src/main/include/constants/SwerveConstants.h b/src/main/include/constants/SwerveConstants.h
@@ -42,6 +42,7 @@ inline constexpr units::ampere_t SLIP_CURRENT_LIMIT = 180_A;
 
 namespace physical {
 
+#ifdef __FRC_ROBORIO__
 inline constexpr units::radian_t IMU_ROLL_OFFSET = -0.4168_deg;
 inline constexpr units::radian_t IMU_PITCH_OFFSET = -89.879_deg;
 inline constexpr units::radian_t IMU_YAW_OFFSET = 89.9885_deg;
@@ -60,6 +61,27 @@ inline constexpr bool FL_DRIVE_INVERT = false;
 inline constexpr bool FR_DRIVE_INVERT = true;
 inline constexpr bool BL_DRIVE_INVERT = false;
 inline constexpr bool BR_DRIVE_INVERT = true;
+#else
+inline constexpr units::radian_t IMU_ROLL_OFFSET = 0_deg;
+inline constexpr units::radian_t IMU_PITCH_OFFSET = 0_deg;
+inline constexpr units::radian_t IMU_YAW_OFFSET = 0_deg;
+
+inline constexpr units::turn_t FL_ENC_OFFSET = .25_tr;
+inline constexpr units::turn_t FR_ENC_OFFSET = .25_tr;
+inline constexpr units::turn_t BL_ENC_OFFSET = .25_tr;
+inline constexpr units::turn_t BR_ENC_OFFSET = .25_tr;
+
+inline constexpr bool FL_STEER_INVERT = false;
+inline constexpr bool FR_STEER_INVERT = false;
+inline constexpr bool BL_STEER_INVERT = false;
+inline constexpr bool BR_STEER_INVERT = false;
+
+inline constexpr bool FL_DRIVE_INVERT = false;
+inline constexpr bool FR_DRIVE_INVERT = false;
+inline constexpr bool BL_DRIVE_INVERT = false;
+inline constexpr bool BR_DRIVE_INVERT = false;
+#endif
+
 
 inline constexpr units::scalar_t STEER_GEARING_MK4I =
     (50.0 / 14.0) * (60.0 / 10.0);