Merge pull request #233 from yapplejack/sparkmax-optimizations

Sparkmax optimizations

src/main/java/swervelib/motors/SparkMaxSwerve.java

@@ -224,12 +224,38 @@ public void configureIntegratedEncoder(double positionConversionFactor)
     {
       configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
       configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+      // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+      // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
+      // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
+      // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+      // This value was taken from:
+      // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
+      // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
+      configureSparkMax(() -> encoder.setMeasurementPeriod(10));
+      configureSparkMax(() -> encoder.setAverageDepth(2));
 
       // Taken from
       // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+      // Unused frames can be set to 65535 to decrease CAN ultilization.
       configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
     } else
     {
+      // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
+      // This needs to be set to 20ms or under to properly update the swerve module position for odometry
+      // Configuration taken from 3005, the team who helped develop the Max Swerve:
+      // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
+      // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. 
+      // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
+      // with limited testing 19ms did not return the same value while the module was constatntly rotating.
+      if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+      {
+        configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
+      }
+      // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
+      else if(absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
+      {
+        configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
+      }
       configureSparkMax(() -> {
         if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
         {