Merge branch 'merge_drive' of https://github.com/SouthwestRoboticsPro…

…gramming/2024_Main_Robot into merge_drive

Robot/.gitignore

@@ -1,5 +1,3 @@
 hs_err_pid*
-simgui.json
-simgui-window.json
-networktables.json
-networktables.json.bak
+simgui*.json
+networktables.json*

Robot/build.gradle

@@ -75,9 +75,6 @@ dependencies {
     implementation project(':MathLib')
     implementation project(':RobotLib')
 
-    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
-    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
-
     implementation 'com.google.code.gson:gson:2.9.0'
 
     implementation wpi.java.deps.wpilib()

Robot/src/main/java/com/swrobotics/robot/logging/AutoLoggedInputs.java

@@ -0,0 +1,202 @@
+package com.swrobotics.robot.logging;
+
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+import java.lang.reflect.Field;
+import java.util.ArrayList;
+import java.util.List;
+
+public abstract class AutoLoggedInputs implements LoggableInputs {
+    private enum LogType {
+        Raw,
+        Boolean,
+        Integer,
+        Long,
+        Float,
+        Double,
+        String,
+        BooleanArray,
+        IntegerArray,
+        LongArray,
+        FloatArray,
+        DoubleArray,
+        StringArray
+    }
+
+    private static final class LogEntry {
+        private final LogType type;
+        private final Field field;
+        private final Object defVal;
+
+        public LogEntry(AutoLoggedInputs in, Field field) {
+            this.field = field;
+
+            Class<?> clazz = field.getType();
+            if (clazz == byte[].class)
+                type = LogType.Raw;
+            else if (clazz == boolean.class || clazz == Boolean.class)
+                type = LogType.Boolean;
+            else if (clazz == int.class || clazz == Integer.class)
+                type = LogType.Integer;
+            else if (clazz == long.class || clazz == Long.class)
+                type = LogType.Long;
+            else if (clazz == float.class || clazz == Float.class)
+                type = LogType.Float;
+            else if (clazz == double.class || clazz == Double.class)
+                type = LogType.Double;
+            else if (clazz == String.class)
+                type = LogType.String;
+            else if (clazz == boolean[].class)
+                type = LogType.BooleanArray;
+            else if (clazz == int[].class)
+                type = LogType.IntegerArray;
+            else if (clazz == long[].class)
+                type = LogType.LongArray;
+            else if (clazz == float[].class)
+                type = LogType.FloatArray;
+            else if (clazz == double[].class)
+                type = LogType.DoubleArray;
+            else if (clazz == String[].class)
+                type = LogType.StringArray;
+            else
+                throw new AssertionError("Field is not a loggable type: " + field);
+
+            try {
+                defVal = field.get(in);
+            } catch (ReflectiveOperationException e) {
+                throw new RuntimeException("Failed to get default value", e);
+            }
+        }
+    }
+
+    private final LogEntry[] entries;
+
+    public AutoLoggedInputs() {
+        List<LogEntry> entries = new ArrayList<>();
+        for (Field field : getClass().getFields()) {
+            entries.add(new LogEntry(this, field));
+        }
+        this.entries = entries.toArray(new LogEntry[0]);
+    }
+
+    private int[] toInts(long[] longs) {
+        int[] ints = new int[longs.length];
+        for (int i = 0; i < ints.length; i++)
+            ints[i] = (int) longs[i];
+        return ints;
+    }
+
+    private long[] toLongs(int[] ints) {
+        long[] longs = new long[ints.length];
+        for (int i = 0; i < ints.length; i++)
+            longs[i] = ints[i];
+        return longs;
+    }
+
+    @Override
+    public void toLog(LogTable table) {
+        for (LogEntry entry : entries) {
+            String name = entry.field.getName();
+            Object val;
+            try {
+                val = entry.field.get(this);
+            } catch (ReflectiveOperationException e) {
+                System.err.println("Failed to log " + entry.field);
+                e.printStackTrace();
+                return;
+            }
+
+            switch (entry.type) {
+                case Raw:
+                    table.put(name, (byte[]) val);
+                    break;
+                case Boolean:
+                    table.put(name, (Boolean) val);
+                    break;
+                case Integer:
+                    table.put(name, (Integer) val);
+                    break;
+                case Long:
+                    table.put(name, (Long) val);
+                    break;
+                case Float:
+                    table.put(name, (Float) val);
+                    break;
+                case Double:
+                    table.put(name, (Double) val);
+                    break;
+                case String:
+                    table.put(name, (String) val);
+                    break;
+                case BooleanArray:
+                    table.put(name, (boolean[]) val);
+                    break;
+                case IntegerArray:
+                    table.put(name, toLongs((int[]) val));
+                    break;
+                case LongArray:
+                    table.put(name, (long[]) val);
+                    break;
+                case DoubleArray:
+                    table.put(name, (double[]) val);
+                    break;
+                case StringArray:
+                    table.put(name, (String[]) val);
+                    break;
+            }
+        }
+    }
+
+    @Override
+    public void fromLog(LogTable table) {
+        for (LogEntry entry : entries) {
+            String name = entry.field.getName();
+            Object val, def = entry.defVal;
+            switch (entry.type) {
+                case Raw:
+                    val = table.getRaw(name, (byte[]) def);
+                    break;
+                case Boolean:
+                    val = table.getBoolean(name, (Boolean) def);
+                    break;
+                case Integer:
+                    val = (int) table.getInteger(name, (Integer) def);
+                    break;
+                case Long:
+                    val = table.getInteger(name, (Long) def);
+                    break;
+                case Double:
+                    val = table.getDouble(name, (Double) def);
+                    break;
+                case String:
+                    val = table.getString(name, (String) def);
+                    break;
+                case BooleanArray:
+                    val = table.getBooleanArray(name, (boolean[]) def);
+                    break;
+                case IntegerArray:
+                    val = toInts(table.getIntegerArray(name, toLongs((int[]) def)));
+                    break;
+                case LongArray:
+                    val = table.getIntegerArray(name, (long[]) def);
+                    break;
+                case DoubleArray:
+                    val = table.getDoubleArray(name, (double[]) def);
+                    break;
+                case StringArray:
+                    val = table.getStringArray(name, (String[]) def);
+                    break;
+                default:
+                    return;
+            }
+
+            try {
+                entry.field.set(this, val);
+            } catch (ReflectiveOperationException e) {
+                System.err.println("Failed to set field " + entry.field);
+                e.printStackTrace();
+            }
+        }
+    }
+}

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java

@@ -47,7 +47,12 @@ public SwerveDrive(FieldInfo fieldInfo) {
             positions[i] = info.position();
         }
 
-        this.kinematics = new SwerveKinematics(positions);
+        double minMax = Double.POSITIVE_INFINITY;
+        for (SwerveModule module : modules) {
+            minMax = Math.min(module.getMaxVelocity(), minMax);
+        }
+
+        this.kinematics = new SwerveKinematics(positions, minMax);
         this.estimator = new SwerveEstimator(fieldInfo);
 
         prevPositions = null;

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java

@@ -101,8 +101,11 @@ public void update(Twist2d driveTwist) {
 
                 List<TagTrackerInput.VisionUpdate> newVisionUpdates = new ArrayList<>();
                 newVisionUpdates.add(visionUpdate);
-                newVisionUpdates.sort(this::compareStdDevs);
+
+                // Insert new update entry for this vision update
                 updates.put(timestamp, new PoseUpdate(prevToVisionTwist, newVisionUpdates));
+
+                // Overwrite nextUpdate with twist after this vision update
                 updates.put(nextUpdate.getKey(), new PoseUpdate(visionToNextTwist, nextUpdate.getValue().visionUpdates));
             }
         }

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveKinematics.java

@@ -11,9 +11,11 @@
 
 public final class SwerveKinematics {
     private final SwerveDriveKinematics kinematics;
+    private final double maxVelocity;
 
-    public SwerveKinematics(Translation2d[] modulePositions) {
+    public SwerveKinematics(Translation2d[] modulePositions, double maxVelocity) {
         kinematics = new SwerveDriveKinematics(modulePositions);
+        this.maxVelocity = maxVelocity;
     }
 
     public SwerveModuleState[] getStates(ChassisSpeeds robotRelSpeeds) {
@@ -32,7 +34,10 @@ public SwerveModuleState[] getStates(ChassisSpeeds robotRelSpeeds) {
                         twistVel.dy / periodicTime,
                         twistVel.dtheta / periodicTime);
 
-        return kinematics.toSwerveModuleStates(robotRelSpeeds);
+        SwerveModuleState[] states = kinematics.toSwerveModuleStates(robotRelSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(states, maxVelocity);
+
+        return states;
     }
 
     public Twist2d getTwistDelta(SwerveModulePosition[] startPositions, SwerveModulePosition[] endPositions) {

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveModule.java

@@ -28,12 +28,12 @@ public Info(CANAllocation.SwerveIDs ids, double x, double y, NTEntry<Double> off
 
     private final Info info;
     private SwerveModuleIO hardwareIO;
-    private SwerveModuleIOInputsAutoLogged inputs;
+    private SwerveModuleIO.SwerveModuleIOInputs inputs;
 
     public SwerveModule(SwerveModuleIO hardwareIO, Info info) {
         this.hardwareIO = hardwareIO;
         this.info = info;
-        inputs = new SwerveModuleIOInputsAutoLogged();
+        inputs = new SwerveModuleIO.SwerveModuleIOInputs();
     }
 
     public void setTargetState(SwerveModuleState targetState) {
@@ -67,6 +67,10 @@ public void update() {
         hardwareIO.updateInputs(inputs);
         Logger.getInstance().processInputs("Drive/" + info.name() + " Module", inputs);
     }
+
+    public double getMaxVelocity() {
+        return hardwareIO.getMaxVelocity();
+    }
 
     private SwerveModuleState optimizeSwerveModuleState(SwerveModuleState targetState) {
         Rotation2d targetAngle = targetState.angle;
@@ -77,7 +81,7 @@ private SwerveModuleState optimizeSwerveModuleState(SwerveModuleState targetStat
             targetVelocity = -targetVelocity;
             targetAngle.plus(Rotation2d.fromDegrees(180));
         }
-
+        
         return new SwerveModuleState(targetVelocity, targetAngle);
     }
 
@@ -115,8 +119,10 @@ private static double placeInAppropriate0To360Scope(double scopeReference, doubl
         }
         return newAngle;
     }
+
 }
 
+
     // TODO
     // /** CTRE-Specific signals directly from the motor controllers and encoders */
     // public BaseStatusSignal[] getSignals() {

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveModuleIO.java

@@ -1,5 +1,6 @@
 package com.swrobotics.robot.subsystems.swerve;
 
+import com.swrobotics.robot.logging.AutoLoggedInputs;
 import org.littletonrobotics.junction.AutoLog;
 
 import com.ctre.phoenix6.BaseStatusSignal;
@@ -26,8 +27,8 @@ default BaseStatusSignal[] getCTRESignals() {
     /**
      * Holds data that can be read from the corresponding swerve drive module IO implementation.
      */
-    @AutoLog
-    class SwerveModuleIOInputs {
+//    @AutoLog
+    class SwerveModuleIOInputs extends AutoLoggedInputs {
         // Drive Inputs
         public double drivePositionRad = 0;
         /**