fix: optimize settings

fix: unit tests for fast kinemaitcs add: swerve generator from 254, prepare to mattlib and make fast/good add: more logging for limit switches add: start work on a faster polynomial regression (broked)
auriium2 · Feb 22, 2024 · 47c8d11 · 47c8d11
1 parent 489786d
commit 47c8d11
Show file tree

Hide file tree

Showing 37 changed files with 813 additions and 70 deletions.
diff --git a/mattlib2-auto/pom.xml → mattlib2-controls/pom.xml b/mattlib2-auto/pom.xml → mattlib2-controls/pom.xml
@@ -5,11 +5,11 @@
     <parent>
         <artifactId>mattlib2</artifactId>
         <groupId>xyz.auriium</groupId>
-        <version>1.6.4-SNAPSHOT</version>
+        <version>1.7.1-SNAPSHOT</version>
     </parent>
     <modelVersion>4.0.0</modelVersion>
 
-    <artifactId>mattlib2-auto</artifactId>
+    <artifactId>mattlib2-controls</artifactId>
 
     <properties>
         <maven.compiler.source>17</maven.compiler.source>
@@ -26,12 +26,12 @@
         <dependency>
             <groupId>xyz.auriium</groupId>
             <artifactId>mattlib2-core</artifactId>
-            <version>1.6.4-SNAPSHOT</version>
+            <version>1.7.1-SNAPSHOT</version>
         </dependency>
         <dependency>
             <groupId>xyz.auriium</groupId>
             <artifactId>mattlib2-hardware</artifactId>
-            <version>1.6.4-SNAPSHOT</version>
+            <version>1.7.1-SNAPSHOT</version>
         </dependency>
         <dependency>
             <groupId>org.ojalgo</groupId>
@@ -40,7 +40,6 @@
         </dependency>
 
 
-
     </dependencies>
 
 </project>
diff --git a/mattlib2-auto/readme.md → mattlib2-controls/readme.md b/mattlib2-auto/readme.md → mattlib2-controls/readme.md
diff --git a/...auriium/mattlib2/auto/RoutineTesting.java → ...auriium/mattlib2/auto/RoutineTesting.java b/...auriium/mattlib2/auto/RoutineTesting.java → ...auriium/mattlib2/auto/RoutineTesting.java
diff --git a/...2/auto/dynamics/FastSwerveKinematics.java → ...2/auto/dynamics/FastSwerveKinematics.java b/...2/auto/dynamics/FastSwerveKinematics.java → ...2/auto/dynamics/FastSwerveKinematics.java
@@ -3,21 +3,26 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import org.ojalgo.matrix.MatrixR064;
-import org.ojalgo.matrix.store.Primitive64Store;
+
+import java.util.Arrays;
 
 public class FastSwerveKinematics {
 
     final MatrixR064 inverseKinematics;
     final MatrixR064 forwardKinematics_pseudo;
 
+    final Rotation2d[] rotationVector = new Rotation2d[4];
+
     public FastSwerveKinematics(MatrixR064 inverseKinematics, MatrixR064 forwardKinematics_pseudo) {
         this.inverseKinematics = inverseKinematics;
         this.forwardKinematics_pseudo = forwardKinematics_pseudo;
+        Arrays.fill(rotationVector, new Rotation2d());
     }
 
-    public FastSwerveKinematics load(Translation2d[] swerveModulePositionOffsets_four) {
+    public static FastSwerveKinematics load(Translation2d[] swerveModulePositionOffsets_four) {
         //Generate 1st order inverse kinematic matrix
 
         var inverseKinematicBuilder = MatrixR064.FACTORY.makeDense(8,3);
@@ -35,26 +40,41 @@ public FastSwerveKinematics load(Translation2d[] swerveModulePositionOffsets_fou
         }
 
         MatrixR064 inverseKinematics = inverseKinematicBuilder.get();
-        MatrixR064 forwardKinematics_pseudo = inverseKinematics.invert();
+        MatrixR064 forwardKinematics_pseudo = (inverseKinematics.transpose().multiply(inverseKinematics)).invert().multiply(inverseKinematics.transpose());
+
+                //FUCKING GIVE ME THE MOORE PERSIJISNINFOSNFOSNFAISNFO
+        //DUMB SHIT
 
         return new FastSwerveKinematics(inverseKinematics, forwardKinematics_pseudo);
     }
 
 
     public SwerveModuleState[] convertCentroidStateToModuleStateSafe(ChassisSpeeds speeds) {
+        if (speeds.vxMetersPerSecond == 0.0
+                && speeds.vyMetersPerSecond == 0.0
+                && speeds.omegaRadiansPerSecond == 0.0) {
+            SwerveModuleState[] moduleStates = new SwerveModuleState[4];
+            for (int i = 0; i < 4; i++) {
+                moduleStates[i] = new SwerveModuleState(0.0, rotationVector[i]);
+            }
+
+            return moduleStates;
+        }
+
         MatrixR064 centroidStateVector = MatrixR064.FACTORY.column(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond);
         MatrixR064 moduleStateVector = convertCentroidStateToModuleState(centroidStateVector); //compiler inlines this
 
         SwerveModuleState[] locallyAllocatedStates = new SwerveModuleState[4]; //TODO reduce object allocations
 
         for (int i = 0; i < 4; i++) {
-            double x = moduleStateVector.doubleValue(2 * i);
-            double y = moduleStateVector.doubleValue(2 * i + 1);
+            double x = moduleStateVector.doubleValue(i * 2);
+            double y = moduleStateVector.doubleValue(i * 2  + 1);
 
             double velocity = Math.hypot(x,y);
             Rotation2d angle = new Rotation2d(x,y);
 
             locallyAllocatedStates[i] = new SwerveModuleState(velocity, angle);
+            rotationVector[i] = angle;
         }
 
         return locallyAllocatedStates;
@@ -73,6 +93,7 @@ public ChassisSpeeds convertModuleStateToCentroidState(SwerveModuleState[] state
                 states[3].speedMetersPerSecond * states[3].angle.getCos()
         );
 
+
         MatrixR064 centroidStateVector = convertModuleStateToCentroidState(moduleStateVector);
 
         return new ChassisSpeeds(