allow inversion of swerve module turn motors

Author: CoolSpy3

src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java

@@ -2,16 +2,16 @@
 
 public final class SwerveConfig {
 
-    public double wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel;
+    public double wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, driveModifier;
     public double[] kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels,
             drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZero;
-    public boolean[] driveInversion, reversed;
+    public boolean[] driveInversion, turnInversion, reversed;
 
     public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu,
             double autoCentripetalAccel, double[] kForwardVolts, double[] kForwardVels, double[] kForwardAccels,
             double[] kBackwardVolts, double[] kBackwardVels, double[] kBackwardAccels, double[] drivekP,
             double[] drivekI, double[] drivekD, double[] turnkP, double[] turnkI, double[] turnkD, double[] turnkS,
-            double[] turnkV, double[] turnkA, double[] turnZero, boolean[] driveInversion, boolean[] reversed) {
+            double[] turnkV, double[] turnkA, double[] turnZero, boolean[] driveInversion, boolean[] reversed, double driveModifier, boolean[] turnInversion) {
         this.wheelDiameterMeters = wheelDiameterMeters;
         this.driveGearing = driveGearing;
         this.mu = mu;
@@ -34,6 +34,8 @@ public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu,
         this.turnZero = turnZero;
         this.driveInversion = driveInversion;
         this.reversed = reversed;
+        this.driveModifier = driveModifier;
+        this.turnInversion = turnInversion;
     }
 
 }

src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java

@@ -42,7 +42,7 @@ public enum ModuleType {FL, FR, BL, BR};
     private SimpleMotorFeedforward forwardSimpleMotorFF, backwardSimpleMotorFF, turnSimpleMotorFeedforward;
     private double lastAngle, maxAchievableTurnVelocityDps, maxAchievableTurnAccelerationMps2, turnToleranceDeg, angleDiff;
 
-    public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCoder turnEncoder, double driveModifier,
+    public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCoder turnEncoder,
                         int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
         //SmartDashboard.putNumber("Target Angle (deg)", 0.0);
         this.timer = new Timer();
@@ -56,6 +56,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
         drive.setInverted(config.driveInversion[arrIndex]);
         drive.getEncoder().setPositionConversionFactor(positionConstant);
         drive.getEncoder().setVelocityConversionFactor(positionConstant / 60);
+        turn.setInverted(config.turnInversion[arrIndex]);
 
         final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */;
         double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2;
@@ -107,7 +108,7 @@ public SwerveModule(SwerveConfig config, ModuleType type, CANSparkMax drive, CAN
         this.turnEncoder = turnEncoder;
         this.turnEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
 
-        this.driveModifier = driveModifier;
+        this.driveModifier = config.driveModifier;
         this.reversed = config.reversed[arrIndex];
         this.turnZero = config.turnZero[arrIndex];