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;
  /**