Add force ffs

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -426,13 +426,22 @@ private void updateVision() {
   }
 
   /**
-   * Runs the modules to the specified ChassisSpeeds (robot velocity)
+   * Runs the modules to the specified ChassisSpeeds (robot velocity) and with the specified force
+   * feedforwards
    *
    * @param speeds the ChassisSpeeds to run the drivetrain at
    * @param openLoop boolean for if the drivetrain should run with feedforward control (open loop)
    *     or with feedback control (closed loop)
+   * @param moduleForcesXNewton the x-components of the force feedforwards. Should be in order FL,
+   *     FR, BL, BR. Should have the same length as the modules array
+   * @param moduleForcesYNewton the y-components of the force feedforwards. Same order and length as
+   *     x-components
    */
-  private void drive(ChassisSpeeds speeds, boolean openLoop) {
+  private void drive(
+      ChassisSpeeds speeds,
+      boolean openLoop,
+      double[] moduleForcesXNetwon,
+      double[] moduleForcesYNewton) {
     // Converts time continuous chassis speeds to setpoints after the specified time (dtSeconds)
     speeds = ChassisSpeeds.discretize(speeds, 0.02);
 
@@ -446,6 +455,9 @@ private void drive(ChassisSpeeds speeds, boolean openLoop) {
 
     SwerveModuleState[] optimizedStates = new SwerveModuleState[modules.length];
 
+    // Instead of storing velocity here we store Newtons of force
+    SwerveModuleState[] forceSetpoints = new SwerveModuleState[modules.length];
+
     for (int i = 0; i < optimizedStates.length; i++) {
       if (openLoop) {
         // Heuristic to enable/disable FOC
@@ -456,14 +468,41 @@ private void drive(ChassisSpeeds speeds, boolean openLoop) {
                         + Math.pow(this.getVelocityRobotRelative().vyMetersPerSecond, 2))
                 < SWERVE_CONSTANTS.getMaxLinearSpeed() * 0.9; // 0.9 is 90% of drivetrain max speed
         optimizedStates[i] = modules[i].runOpenLoop(states[i], focEnable);
+
+        // Needs some value to update
+        forceSetpoints[i] = new SwerveModuleState();
       } else {
-        optimizedStates[i] = modules[i].runClosedLoop(states[i]);
+
+        double robotRelForceX =
+            moduleForcesXNetwon[i] * getRotation().unaryMinus().getCos()
+                - moduleForcesYNewton[i] * getRotation().unaryMinus().getSin();
+        double robotRelForceY =
+            moduleForcesXNetwon[i] * getRotation().unaryMinus().getSin()
+                + moduleForcesYNewton[i] * getRotation().unaryMinus().getCos();
+
+        forceSetpoints[i] =
+            new SwerveModuleState(
+                Math.hypot(robotRelForceX, robotRelForceY),
+                new Rotation2d(robotRelForceX, robotRelForceY));
+
+        optimizedStates[i] = modules[i].runClosedLoop(states[i], robotRelForceX, robotRelForceY);
       }
     }
-
+    Logger.recordOutput("SwerveStates/Force Setpoints", forceSetpoints);
     Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedStates);
   }
 
+  /**
+   * Runs the modules to the specified ChassisSpeeds (robot velocity)
+   *
+   * @param speeds the ChassisSpeeds to run the drivetrain at
+   * @param openLoop boolean for if the drivetrain should run with feedforward control (open loop)
+   *     or with feedback control (closed loop)
+   */
+  private void drive(ChassisSpeeds speeds, boolean openLoop) {
+    drive(speeds, openLoop, new double[] {0.0, 0.0, 0.0, 0.0}, new double[] {0.0, 0.0, 0.0, 0.0});
+  }
+
   /**
    * Drive closed-loop at robot relative speeds
    *
@@ -1008,7 +1047,11 @@ public Consumer<SwerveSample> choreoDriveController() {
               sample.getChassisSpeeds().plus(feedback), getPose().getRotation());
       Logger.recordOutput("Choreo/Target Speeds Robot Relative", speeds);
 
-      this.drive(speeds, false);
+      // These are field relative i believe
+      double[] moduleForcesX = sample.moduleForcesX();
+      double[] moduleForcesY = sample.moduleForcesY();
+
+      this.drive(speeds, false, moduleForcesX, moduleForcesY);
     };
   }
 

src/main/java/frc/robot/subsystems/swerve/module/Module.java

@@ -62,6 +62,7 @@ public void setTurnVoltage(double volts) {
     io.setTurnVoltage(volts);
   }
 
+  /** Stops the module */
   public void stop() {
     io.setDriveVoltage(0);
   }
@@ -73,10 +74,37 @@ public void stop() {
    * @return the optimized state
    */
   public SwerveModuleState runClosedLoop(SwerveModuleState state) {
+    return runClosedLoop(state, 0.0, 0.0);
+  }
+
+  /**
+   * Runs closed loop to the specified state, with a force feedforward. Used for automated actions,
+   * like auto.
+   *
+   * @param state setpoint state
+   * @param forceXNewtons the x-component of the requested force vector
+   * @param forceYNewtons the y-component of the requested force vector
+   * @return the optimized state
+   */
+  public SwerveModuleState runClosedLoop(
+      SwerveModuleState state, double forceXNewtons, double forceYNewtons) {
     state.optimize(getAngle());
 
+    // Force on the motor is the total force vector projected onto the velocity vector
+    // to project a onto b take ||a||*cos(theta) where theta is the angle between the two vectors
+    // We want the magnitude of the projection, so we can ignore the direction of this later
+    final double thetaRads =
+        Math.atan2(forceYNewtons, forceXNewtons) - inputs.turnPosition.getRadians();
+    double forceNewtons = Math.hypot(forceXNewtons, forceYNewtons) * Math.cos(thetaRads);
+    if (Math.signum(forceNewtons) * Math.signum(state.speedMetersPerSecond) < 0) {
+      forceNewtons = 0;
+    }
+
     io.setTurnPositionSetpoint(state.angle);
-    io.setDriveVelocitySetpoint(state.speedMetersPerSecond);
+    io.setDriveVelocitySetpoint(state.speedMetersPerSecond, forceNewtons);
+
+    Logger.recordOutput(
+        "Swerve/" + constants.prefix + " Module/Force Feedforward Newtons", forceNewtons);
 
     return state;
   }

src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java

@@ -214,7 +214,17 @@ public void setDriveVoltage(double volts) {
   }
 
   public void setDriveVelocitySetpoint(double setpointMetersPerSecond) {
-    driveTalon.setControl(driveVelocityControl.withVelocity(setpointMetersPerSecond));
+    setDriveVelocitySetpoint(setpointMetersPerSecond, 0.0);
+  }
+
+  public void setDriveVelocitySetpoint(double setpointMetersPerSecond, double forceNewtons) {
+    // Convert the requested force to the current required to output that force
+    // Conversion factor: Stall torque / Stall current (aka kT)
+    double forceFFAmps = forceNewtons * 9.37 / 483;
+    // Kelpie had no conversion from force to torque. maybe we should add this?? idk? comment claims
+    // its handled in sensor to mech ratio
+    driveTalon.setControl(
+        driveVelocityControl.withVelocity(setpointMetersPerSecond).withFeedForward(forceFFAmps));
   }
 
   public void setTurnVoltage(double volts) {