DriveFFController using bilinear regression

src/main/java/frc/robot/commands/shooter/ShooterWindup.java

@@ -1,7 +1,6 @@
 package frc.robot.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.subsystems.shooter.pivot.flywheel.Flywheel;
 
 public class ShooterWindup extends Command {

src/main/java/frc/robot/subsystems/drivetrain/swerve/DriveFFController.java

@@ -0,0 +1,123 @@
+package frc.robot.subsystems.drivetrain.swerve;
+
+import java.util.ArrayList;
+
+public class DriveFFController {
+    ArrayList<double[]> points = new ArrayList<double[]>();
+    public DriveFFController() {
+        // drive meters per second, radians per second, mps error
+        points.add(new double[] {0, 0, 0});
+        points.add(new double[] {1, 0, 0});
+        points.add(new double[] {2, 0, 0});
+        points.add(new double[] {0, Math.PI, 0});
+        points.add(new double[] {0, 2 * Math.PI, 0});
+        points.add(new double[] {2, 2 * Math.PI, 0.45});
+        points.add(new double[] {1, 2 * Math.PI, 0.212});
+        points.add(new double[] {2, Math.PI, 0.344});
+        points.add(new double[] {1, Math.PI, 0.25});
+    }
+
+    public double calculate(double imps, double irps) {
+        double mps = Math.abs(imps);
+        double rps = Math.abs(irps);
+        for (int i = 0; i < points.size(); i++) {
+            if (points.get(i)[0] == mps && points.get(i)[1] == rps) {
+                return points.get(i)[2];
+            }
+        }
+
+        double[] inputPoint = new double[] {mps, rps, 0};
+
+        double[][] p = new double[4][3];
+        for (int i = 0; i < 4; i++) {
+            p[i] = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, -1};
+        }
+
+        ArrayList<double[]> temp = points;
+        for (int n = 0; n < 4; n++) {
+            int index = 0;
+            for(int i = 0; i < temp.size(); i++) {
+                if (distance(temp.get(i), inputPoint) < distance(p[n], inputPoint)) {
+                    boolean good = true;
+                    for (int m = 0; m < n; m++) {
+                        if (distance(p[m], temp.get(i)) > Math.sqrt(2) + 0.1) {
+                            good = false;
+                        }
+                    }
+                    if (good) {
+                        p[n] = temp.get(i);
+                        index = i;
+                    }
+                }
+            }
+            temp.remove(index);
+        }
+
+        double[] tr, tl, br, bl;
+        tr = new double[] {-Integer.MAX_VALUE, -Integer.MAX_VALUE, 0};
+        tl = new double[] {Integer.MAX_VALUE, -Integer.MAX_VALUE, 0};
+        br = new double[] {-Integer.MAX_VALUE, Integer.MAX_VALUE, 0};
+        bl = new double[] {Integer.MAX_VALUE, Integer.MAX_VALUE, 0};
+
+        boolean trT = false;
+        boolean tlT = false;
+        boolean brT = false;
+        boolean blT = false;
+
+
+        for (int i = 0; i < 4; i++) {
+            if (p[i][0] >= inputPoint[0] && p[i][1] >= inputPoint[1] && !trT) {
+                tr = p[i];
+                trT = true;
+            }
+            else if (p[i][0] <= inputPoint[0] && p[i][1] >= inputPoint[1] && !tlT) {
+                tl = p[i];
+                tlT = true;
+            }
+            else if (p[i][0] >= inputPoint[0] && p[i][1] <= inputPoint[1] && !brT) {
+                br = p[i];
+                brT = true;
+            }
+            else if (p[i][0] <= inputPoint[0] && p[i][1] <= inputPoint[1] && !blT) {
+                bl = p[i];
+                blT = true;
+            }
+        }
+
+        double r1 = (tr[0] - inputPoint[0])/(tr[0] - tl[0]) * tl[2] + (inputPoint[0] - tl[0])/(tr[0] - tl[0]) * tr[2];
+        double r2 = (br[0] - inputPoint[0])/(br[0] - bl[0]) * bl[2] + (inputPoint[0] - bl[0])/(br[0] - bl[0]) * br[2];
+        double pf = (inputPoint[1] - br[1])/(tr[1] - br[1]) * r1 + (tr[1] - inputPoint[1])/(tr[1] - bl[1]) * r2;
+
+        if (Double.isNaN(r1)) {
+            return r2;
+        }
+        if (Double.isNaN(r2)) {
+            return r1;
+        }
+
+        if (mps >= 0 && rps >= 0) {
+            return pf;
+        }
+
+        if (mps >= 0 && rps <= 0) {
+            return -pf;
+        }
+
+        if (mps <= 0 && rps >= 0) {
+            return -pf;
+        }
+
+        return pf;
+
+    }
+
+    private double distance(double[] a, double[] b) {
+        return Math.sqrt(Math.pow(a[0] - b[0], 2) + Math.pow((a[1] - b[1]) / Math.PI, 2));
+    }
+
+    public static void main(String[] args) {
+        DriveFFController driveFFController = new DriveFFController();
+
+        System.out.println(driveFFController.calculate(1, 1));
+    }
+}

src/main/java/frc/robot/subsystems/drivetrain/swerve/SwerveDrive.java

@@ -8,12 +8,14 @@
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.InterpolatingMatrixTreeMap;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -57,10 +59,10 @@ public class SwerveDrive extends SubsystemBase{
     private ChassisSpeeds measuredRobotRelativeSpeeds = new ChassisSpeeds(0,0,0);
     private ChassisSpeeds measuredFeildRelativeSpeeds = new ChassisSpeeds(0,0,0);
 
-    private PIDController m_angleFeedbackController = new PIDController(0.00, 0.0, 0.000);
-    private SimpleMotorFeedforward m_angleFeedForwardController = new SimpleMotorFeedforward(0, 1);
+    private PIDController m_angleFeedbackController;
+    private DriveFFController driveFFController;
 
-    private PIDController m_translationalFeedbackController = new PIDController(0.00, 0.0, 0.000);
+    private PIDController m_translationalFeedbackController;
 
     private SwerveModuleState[] measuredModuleStates = {
         new SwerveModuleState(),
@@ -81,6 +83,8 @@ public SwerveDrive() {
                     new ModuleIOSim()
                 };
                 gyroIO = new GyroIO() {};
+
+
                 break;
             default:
                 modules = new ModuleIO[] {
@@ -182,16 +186,18 @@ public void periodic() {
             m_poseEstimator.getEstimatedPosition().getRotation().getRadians()
         });
 
+        desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredFeildRelativeSpeeds, yaw);
         Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds);
         Logger.recordOutput("SwerveDrive/measuredRobotRelativeSpeeds", measuredRobotRelativeSpeeds);
         Logger.recordOutput("SwerveDrive/desiredFeildRelativeSpeeds", desiredFeildRelativeSpeeds);
         Logger.recordOutput("SwerveDrive/measuredFeildRelativeSpeeds", measuredFeildRelativeSpeeds);
 
 
         ChassisSpeeds correctedSpeeds = desiredRobotRelativeSpeeds;
-        correctedSpeeds.omegaRadiansPerSecond = 
-            m_angleFeedForwardController.calculate(desiredRobotRelativeSpeeds.omegaRadiansPerSecond, Math.signum(desiredRobotRelativeSpeeds.omegaRadiansPerSecond - measuredRobotRelativeSpeeds.omegaRadiansPerSecond)) + 
-            m_angleFeedbackController.calculate(measuredRobotRelativeSpeeds.omegaRadiansPerSecond, desiredRobotRelativeSpeeds.omegaRadiansPerSecond);
+        // correctedSpeeds.omegaRadiansPerSecond = 
+        //     m_angleFeedForwardController.calculate(desiredRobotRelativeSpeeds.omegaRadiansPerSecond, Math.signum(desiredRobotRelativeSpeeds.omegaRadiansPerSecond - measuredRobotRelativeSpeeds.omegaRadiansPerSecond)) + 
+        //     m_angleFeedbackController.calculate(measuredRobotRelativeSpeeds.omegaRadiansPerSecond, desiredRobotRelativeSpeeds.omegaRadiansPerSecond);
+
 
         Logger.recordOutput("SwerveDrive/correctedSpeeds", correctedSpeeds);
 
@@ -206,7 +212,6 @@ public void periodic() {
 
     public void driveFieldRelative(ChassisSpeeds speeds) {
         desiredFeildRelativeSpeeds = speeds;
-        desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, yaw);
     }
 
     public void driveRobotRelatve(ChassisSpeeds speeds) {