Fixed the conversion factor for talonfx's. Added new command generators.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

src/main/deploy/swerve/falcon/modules/physicalproperties.json

@@ -1,7 +1,7 @@
 {
   "conversionFactor": {
-    "angle": 0.01373291015625,
-    "drive": 1.914649238933429E-5
+	"angle": 28.125,
+	"drive": 0.047286787200699704
   },
   "currentLimit": {
     "drive": 40,

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

@@ -11,6 +11,7 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 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.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -21,6 +22,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.io.File;
+import java.util.function.DoubleSupplier;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.math.SwerveMath;
@@ -76,6 +78,7 @@ public SwerveSubsystem(File directory)
     }
     swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle.
 
+    swerveDrive.setGyroOffset(new Rotation3d(0, 0, Units.degreesToRadians(90)));
     setupPathPlanner();
   }
 
@@ -104,12 +107,12 @@ public void setupPathPlanner()
                                          // Default path replanning config. See the API for the options here
         ),
         () -> {
-                    // Boolean supplier that controls when the path will be mirrored for the red alliance
-                    // This will flip the path being followed to the red side of the field.
-                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
-                    var alliance = DriverStation.getAlliance();
-                    return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
-                },
+          // Boolean supplier that controls when the path will be mirrored for the red alliance
+          // This will flip the path being followed to the red side of the field.
+          // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+          var alliance = DriverStation.getAlliance();
+          return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false;
+        },
         this // Reference to this subsystem to set requirements
                                   );
   }
@@ -135,6 +138,45 @@ public Command getAutonomousCommand(String pathName, boolean setOdomToStart)
     return AutoBuilder.followPath(path);
   }
 
+  /**
+   * Command to drive the robot using translative values and heading as a setpoint.
+   *
+   * @param translationX Translation in the X direction.
+   * @param translationY Translation in the Y direction.
+   * @param headingX     Heading X to calculate angle of the joystick.
+   * @param headingY     Heading Y to calculate angle of the joystick.
+   * @return Drive command.
+   */
+  public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX,
+                              DoubleSupplier headingY)
+  {
+    return run(() -> {
+      // Make the robot move
+      driveFieldOriented(getTargetSpeeds(translationX.getAsDouble(), translationY.getAsDouble(),
+                                         headingX.getAsDouble(),
+                                         headingY.getAsDouble()));
+    });
+  }
+
+  /**
+   * Command to drive the robot using translative values and heading as angular velocity.
+   *
+   * @param translationX     Translation in the X direction.
+   * @param translationY     Translation in the Y direction.
+   * @param angularRotationX Rotation of the robot to set
+   * @return Drive command.
+   */
+  public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX)
+  {
+    return run(() -> {
+      // Make the robot move
+      swerveDrive.drive(new Translation2d(translationX.getAsDouble() * maximumSpeed, translationY.getAsDouble()),
+                        angularRotationX.getAsDouble() * swerveDrive.swerveController.config.maxAngularVelocity,
+                        true,
+                        false);
+    });
+  }
+
   /**
    * Construct the swerve drive.
    *
@@ -301,7 +343,8 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin
   }
 
   /**
-   * Get the chassis speeds based on controller input of 1 joystick and one angle.
+   * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of
+   * 90deg.
    *
    * @param xInput X joystick input for the robot to move in the X direction.
    * @param yInput Y joystick input for the robot to move in the Y direction.
@@ -315,7 +358,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an
     return swerveDrive.swerveController.getTargetSpeeds(xInput,
                                                         yInput,
                                                         angle.getRadians(),
-                                                        getHeading().getRadians(),
+                                                        getHeading().getRadians() - Units.degreesToRadians(90),
                                                         maximumSpeed);
   }
 

src/main/java/swervelib/motors/TalonFXSwerve.java

@@ -154,7 +154,7 @@ public void configureIntegratedEncoder(double positionConversionFactor)
     TalonFXConfigurator cfg = motor.getConfigurator();
     cfg.refresh(configuration);
 
-    positionConversionFactor = 1 / positionConversionFactor;
+    //  positionConversionFactor = 1 / positionConversionFactor;
 
     configuration.MotionMagic = configuration.MotionMagic
         .withMotionMagicCruiseVelocity(100 / positionConversionFactor)