Merge pull request #539 from truher/main

removed most uses of arrays for swerve, instead there are containers with "frontLeft" etc.  also some estimator work.

lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/DriveInALittleSquare.java

@@ -7,6 +7,7 @@
 import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates;
 import org.team100.lib.profile.TrapezoidProfile100;
 import org.team100.lib.state.State100;
 import org.team100.lib.util.Util;
@@ -95,12 +96,12 @@ public void execute() {
         }
 
         // there are four states here because state is mutable :-(
-        SwerveModuleState100[] states = new SwerveModuleState100[] {
+        SwerveModuleStates states = new SwerveModuleStates(
                 new SwerveModuleState100(m_setpoint.v(), Optional.of(m_goal)),
                 new SwerveModuleState100(m_setpoint.v(), Optional.of(m_goal)),
                 new SwerveModuleState100(m_setpoint.v(), Optional.of(m_goal)),
                 new SwerveModuleState100(m_setpoint.v(), Optional.of(m_goal))
-        };
+        );
         m_swerve.setRawModuleStates(states);
     }
 

lib/src/main/java/org/team100/lib/commands/drivetrain/for_testing/OscillateDirect.java

@@ -10,6 +10,7 @@
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.SwerveState;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates;
 import org.team100.lib.util.ParabolicWave;
 import org.team100.lib.util.SquareWave;
 import org.team100.lib.util.TriangleWave;
@@ -107,48 +108,48 @@ public void execute() {
     }
 
     void straight(double speedM_S) {
-        m_swerve.setRawModuleStates(new SwerveModuleState100[] {
+        m_swerve.setRawModuleStates(new SwerveModuleStates (
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // FL
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // FR
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // RL
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)) // RR
-        });
+        ));
     }
 
     void sideways(double speedM_S) {
-        m_swerve.setRawModuleStates(new SwerveModuleState100[] {
+        m_swerve.setRawModuleStates(new SwerveModuleStates (
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)), // FL
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)), // FR
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)), // RL
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotation90)) // RR
-        });
+        ));
     }
 
     void back(double speedM_S) {
-        m_swerve.setRawModuleStates(new SwerveModuleState100[] {
+        m_swerve.setRawModuleStates(new SwerveModuleStates (
                 new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // FL
                 new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // FR
                 new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // RL
                 new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)) // RR
-        });
+        ));
     }
 
     void skew(double speedM_S) {
-        m_swerve.setRawModuleStates(new SwerveModuleState100[] {
+        m_swerve.setRawModuleStates(new SwerveModuleStates (
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // FL
                 new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)), // FR
                 new SwerveModuleState100(speedM_S, Optional.of(GeometryUtil.kRotationZero)), // RL
                 new SwerveModuleState100(-speedM_S, Optional.of(GeometryUtil.kRotation180)) // RR
-        });
+        ));
     }
 
     void spin(double speedM_S) {
-        m_swerve.setRawModuleStates(new SwerveModuleState100[] {
+        m_swerve.setRawModuleStates(new SwerveModuleStates (
                 new SwerveModuleState100(speedM_S, Optional.of(new Rotation2d(-Math.PI / 4))), // FL
                 new SwerveModuleState100(speedM_S, Optional.of(new Rotation2d(-3 * Math.PI / 4))), // FR
                 new SwerveModuleState100(speedM_S, Optional.of(new Rotation2d(Math.PI / 4))), // RL
                 new SwerveModuleState100(speedM_S, Optional.of(new Rotation2d(3 * Math.PI / 4))) // RR
-        });
+        ));
     }
 
     @Override

lib/src/main/java/org/team100/lib/commands/drivetrain/manual/DriveManually.java

@@ -8,7 +8,7 @@
 import org.team100.lib.hid.DriverControl;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.SwerveState;
-import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates;
 import org.team100.lib.swerve.SwerveSetpoint;
 import org.team100.lib.util.NamedChooser;
 
@@ -67,7 +67,7 @@ public void initialize() {
         // Note this is not necessarily "at rest," because we might start driving
         // manually while the robot is moving.
         ChassisSpeeds currentSpeeds = m_drive.getState().chassisSpeeds();
-        SwerveModuleState100[] currentStates = m_drive.getSwerveLocal().states();
+        SwerveModuleStates currentStates = m_drive.getSwerveLocal().states();
         SwerveSetpoint setpoint = new SwerveSetpoint(currentSpeeds, currentStates);
         m_drive.resetSetpoint(setpoint);
         Pose2d p = m_drive.getState().pose();

lib/src/main/java/org/team100/lib/commands/drivetrain/manual/ModuleStateDriver.java

@@ -2,13 +2,13 @@
 
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.hid.DriverControl;
-import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates;
 
 
 public interface ModuleStateDriver extends Glassy {
     /**
      * @param input control units [-1,1]
      * @return module states
      */
-    SwerveModuleState100[] apply(DriverControl.Velocity input);
+    SwerveModuleStates apply(DriverControl.Velocity input);
 }

lib/src/main/java/org/team100/lib/commands/drivetrain/manual/SimpleManualModuleStates.java

@@ -9,6 +9,7 @@
 import org.team100.lib.logging.LoggerFactory.Rotation2dLogger;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 
@@ -37,19 +38,19 @@ public SimpleManualModuleStates(LoggerFactory parent, SwerveKinodynamics swerveK
      * There's no conflict between translation and rotation velocities in this mode.
      */
     @Override
-    public SwerveModuleState100[] apply(DriverControl.Velocity input) {
+    public SwerveModuleStates apply(DriverControl.Velocity input) {
         // dtheta is from [-1, 1], so angle is [-pi, pi]
         Optional<Rotation2d> angle = Optional.of(Rotation2d.fromRadians(Math.PI * input.theta()));
         double speedM_S = m_swerveKinodynamics.getMaxDriveVelocityM_S() * input.x();
         m_log_speed.log( () -> speedM_S);
         m_log_angle.log( angle::get);
         // System.out.println("speed " + speedM_S);
 
-        return new SwerveModuleState100[] {
+        return new SwerveModuleStates (
                 new SwerveModuleState100(speedM_S, angle),
                 new SwerveModuleState100(speedM_S, angle),
                 new SwerveModuleState100(speedM_S, angle),
                 new SwerveModuleState100(speedM_S, angle)
-        };
+        );
     }
 }

lib/src/main/java/org/team100/lib/localization/InterpolationRecord.java

@@ -1,11 +1,10 @@
 package org.team100.lib.localization;
 
-import java.util.Arrays;
 import java.util.Objects;
 
 import org.team100.lib.motion.drivetrain.SwerveState;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveDriveKinematics100;
-import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePositions;
 import org.team100.lib.util.DriveUtil;
 
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -29,7 +28,7 @@ class InterpolationRecord implements Interpolatable<InterpolationRecord> {
     final Rotation2d m_gyroAngle;
 
     // The current encoder readings.
-    final SwerveModulePosition100[] m_wheelPositions;
+    final SwerveModulePositions m_wheelPositions;
 
     /**
      * Constructs an Interpolation Record with the specified parameters.
@@ -43,14 +42,11 @@ class InterpolationRecord implements Interpolatable<InterpolationRecord> {
             SwerveDriveKinematics100 kinematics,
             SwerveState state,
             Rotation2d gyro,
-            SwerveModulePosition100[] wheelPositions) {
+            SwerveModulePositions wheelPositions) {
         m_kinematics = kinematics;
         m_state = state;
         m_gyroAngle = gyro;
-        m_wheelPositions = new SwerveModulePosition100[wheelPositions.length];
-        for (int i = 0; i < wheelPositions.length; ++i) {
-            m_wheelPositions[i] = wheelPositions[i].copy();
-        }
+        m_wheelPositions = new SwerveModulePositions(wheelPositions);
     }
 
     /**
@@ -76,10 +72,12 @@ public InterpolationRecord interpolate(InterpolationRecord endValue, double t) {
             return endValue;
         }
         // Find the new wheel distances.
-        SwerveModulePosition100[] wheelLerp = new SwerveModulePosition100[m_wheelPositions.length];
-        for (int i = 0; i < m_wheelPositions.length; ++i) {
-            wheelLerp[i] = m_wheelPositions[i].interpolate(endValue.m_wheelPositions[i], t);
-        }
+        SwerveModulePositions wheelLerp = new SwerveModulePositions(
+            m_wheelPositions.frontLeft().interpolate(endValue.m_wheelPositions.frontLeft(), t),
+            m_wheelPositions.frontRight().interpolate(endValue.m_wheelPositions.frontRight(), t),
+            m_wheelPositions.rearLeft().interpolate(endValue.m_wheelPositions.rearLeft(), t),
+            m_wheelPositions.rearRight().interpolate(endValue.m_wheelPositions.rearRight(), t)        
+        );
 
         // Find the new gyro angle.
         Rotation2d gyroLerp = m_gyroAngle.interpolate(endValue.m_gyroAngle, t);
@@ -119,7 +117,7 @@ public int hashCode() {
     @Override
     public String toString() {
         return "InterpolationRecord [m_poseMeters=" + m_state + ", m_gyroAngle=" + m_gyroAngle
-                + ", m_wheelPositions=" + Arrays.toString(m_wheelPositions) + "]";
+                + ", m_wheelPositions=" + m_wheelPositions + "]";
     }
 
 }

lib/src/main/java/org/team100/lib/localization/SwerveDrivePoseEstimator100.java

@@ -14,8 +14,8 @@
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeDelta;
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
-import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDelta;
-import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePosition100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleDeltas;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModulePositions;
 import org.team100.lib.util.DriveUtil;
 import org.team100.lib.util.Util;
 
@@ -28,7 +28,6 @@ public class SwerveDrivePoseEstimator100 implements PoseEstimator100, Glassy {
     // look back a little to get a pose for velocity estimation
     private static final double velocityDtS = 0.02;
 
-    private final int m_numModules;
     private final SwerveKinodynamics m_kinodynamics;
     private final TimeInterpolatableBuffer100<InterpolationRecord> m_poseBuffer;
     // LOGGERS
@@ -52,11 +51,10 @@ public SwerveDrivePoseEstimator100(
             LoggerFactory parent,
             SwerveKinodynamics kinodynamics,
             Rotation2d gyroAngle,
-            SwerveModulePosition100[] modulePositions,
+            SwerveModulePositions modulePositions,
             Pose2d initialPoseMeters,
             double timestampSeconds) {
         LoggerFactory child = parent.child(this);
-        m_numModules = modulePositions.length;
         m_kinodynamics = kinodynamics;
         m_poseBuffer = new TimeInterpolatableBuffer100<>(
                 child,
@@ -84,12 +82,10 @@ public SwerveState get(double timestampSeconds) {
     /** Empty the buffer and add the given measurements. */
     public void reset(
             Rotation2d gyroAngle,
-            SwerveModulePosition100[] modulePositions,
+            SwerveModulePositions modulePositions,
             Pose2d pose,
             double timestampSeconds) {
 
-        checkLength(modulePositions);
-
         m_gyroOffset = pose.getRotation().minus(gyroAngle);
 
         // empty the buffer and add the current pose
@@ -173,7 +169,7 @@ public void put(
         for (Map.Entry<Double, InterpolationRecord> entry : m_poseBuffer.tailMap(timestampS, false).entrySet()) {
             double entryTimestampS = entry.getKey();
             Rotation2d entryGyroAngle = entry.getValue().m_gyroAngle;
-            SwerveModulePosition100[] wheelPositions = entry.getValue().m_wheelPositions;
+            SwerveModulePositions wheelPositions = entry.getValue().m_wheelPositions;
             put(entryTimestampS, entryGyroAngle, wheelPositions);
         }
 
@@ -186,8 +182,7 @@ public void put(
     public void put(
             double currentTimeS,
             Rotation2d gyroAngle,
-            SwerveModulePosition100[] wheelPositions) {
-        checkLength(wheelPositions);
+            SwerveModulePositions wheelPositions) {
 
         // the extra little bit here is to make sure we catch the most recent entry even
         // though the clock jitters a little.
@@ -210,7 +205,7 @@ public void put(
         InterpolationRecord value = lowerEntry.getValue();
         SwerveState previousState = value.m_state;
 
-        SwerveModuleDelta[] modulePositionDelta = DriveUtil.modulePositionDelta(
+        SwerveModuleDeltas modulePositionDelta = DriveUtil.modulePositionDelta(
                 value.m_wheelPositions,
                 wheelPositions);
 
@@ -270,13 +265,6 @@ public void put(
 
     ///////////////////////////////////////
 
-    private void checkLength(SwerveModulePosition100[] modulePositions) {
-        int ct = modulePositions.length;
-        if (ct != m_numModules) {
-            throw new IllegalArgumentException("Wrong module count: " + ct);
-        }
-    }
-
     /**
      * Given q and r stddev's, what mixture should that yield?
      * This is the "closed form Kalman gain for continuous Kalman filter with A = 0

lib/src/main/java/org/team100/lib/motion/drivetrain/SwerveDriveSubsystem.java

@@ -13,7 +13,7 @@
 import org.team100.lib.logging.LoggerFactory.FieldRelativeVelocityLogger;
 import org.team100.lib.logging.LoggerFactory.SwerveStateLogger;
 import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity;
-import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleState100;
+import org.team100.lib.motion.drivetrain.kinodynamics.SwerveModuleStates;
 import org.team100.lib.sensors.Gyro;
 import org.team100.lib.swerve.SwerveSetpoint;
 import org.team100.lib.util.Memo;
@@ -147,16 +147,9 @@ public void setChassisSpeedsNormally(ChassisSpeeds speeds) {
     }
 
     /**
-     * array order:
-     * 
-     * frontLeft
-     * frontRight
-     * rearLeft
-     * rearRight
-     * 
      * Does not desaturate.
      */
-    public void setRawModuleStates(SwerveModuleState100[] states) {
+    public void setRawModuleStates(SwerveModuleStates states) {
         m_swerveLocal.setRawModuleStates(states);
     }