Timing inconsistencies and performance issues (#85)

* Added PhotonVision

* Added STD DEVS to photonvision

* Added camera transforms

* SysId Swerve. Fix Swerve Gyro Issue

* Photonvision works

* Fixed Photonvision

* Removed sham cameras

* Initial pass at a PV implementation rewrite/refactor. Should work in real and sim, not sure if it works in replay yet, but it should.

* Initial pass at note tracking, no functionality yet, just logging detected note poses.

* Added note tracking vision IO Inputs

* Drive to Maybe Pose Autonomote Note Tracking!!

* Drive to Maybe Pose Autonomous Note Tracking!!

* better name

* Fixed logs being overwritten

* auto note track

* my computer is too laggy to run so i gotta sim on laptop

* stuff

* photonvision is disabled. janky note detection auto

* getX() replaced with getY()

* Stole from 254

* random translation for note tracking camera

* changed note pose calculation

* it works :)

* Test adding two translations together on real robot.

* i wanna make teleop assist

* probably not working teleop note lineup assist.

* i cooked :(

* this kinda sucks :|

* Added way to disable PV. Line up to note seems to work. Need to make it assist rotation.

* Got note assist during teleop working. Added timestamp check to note tracking camera.

* Reordered teleop note align code.

* Moved lineup pose into inner optional present scope for autoalign

* Tried note tracking auto. Added rumble to both controllers on note pickup. Disabled auto align to note in teleop.

* post de

* Removed assisted rotation from teleopAssitedLineup

* note tracking failures.

* allegedly this solves the problems.

* added distance offset

* workingish note distance

* idk why it wont get the flipped one 😭😭😭😭😭😭😭

* extra auto path

* Goated changes sim

* maybe PV replay?

* some auto position reset stuff

* seems to work okay.

* Removed mega cooked autos. Gave up on note tracking 😭

* READY FOR GRITS DAY 2

* added goated auto v2

* i hate this variable. changed from sim to real.

* qual 47 and whatever next is

* grits

* Added an option for the robot to not intake a note in Sim. Figured out why the robot hit the stage at Grits. It resumed a command that had a waitfor from earlier in the auto. This wasnt a problem earlier in the season since ig no one ever stole our second note. The solution is to add a withTimeout to the Command Group.

---------

Co-authored-by: harry.chen <[email protected]>

build.gradle

@@ -1,3 +1,5 @@
+import groovy.json.JsonSlurper
+
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2024.3.2"
@@ -15,6 +17,8 @@ java {
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
+final MAX_JAVA_HEAP_SIZE_MB = 100;
+
 repositories {
     mavenLocal()
     mavenCentral()
@@ -55,6 +59,15 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                    jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
+
+                    // Set the minimum heap size to the maximum heap size to avoid resizing
+                    jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    jvmArgs.add("-XX:GCTimeRatio=5")
+                    jvmArgs.add("-XX:+UseSerialGC")
+                    jvmArgs.add("-XX:MaxGCPauseMillis=50")
+                    jvmArgs.add("-XX:+AlwaysPreTouch")
                 }
 
                 // Static files artifact
@@ -102,7 +115,7 @@ dependencies {
     testImplementation "org.mockito:mockito-core:5.4.0"
     testImplementation 'org.mockito:mockito-junit-jupiter:5.4.0'
 
-    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    def akitJson = new JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 }