Merge branch 'master' of https://github.com/Team4388/Swerve-Drive

src/main/java/frc4388/robot/Constants.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 
 /**
@@ -51,6 +52,12 @@ public final class Constants {
         public static final float RIGHT_FRONT_ENCODER_OFFSET = 0;   
         public static final float LEFT_BACK_ENCODER_OFFSET = 0;     
         public static final float RIGHT_BACK_ENCODER_OFFSET = 0;
+
+        // swerve PID constants
+        public static final int SWERVE_SLOT_IDX = 0;
+        public static final int SWERVE_PID_LOOP_IDX = 1;
+        public static final int SWERVE_TIMEOUT_MS = 30;
+        public static final Gains SWERVE_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0);
     }
     public static final class LEDConstants {
         public static final int LED_SPARK_ID = 0;

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -32,7 +32,8 @@ public class SwerveDrive extends SubsystemBase
     private CANCoder m_leftFrontEncoder; 
     private CANCoder m_rightFrontEncoder;
     private CANCoder m_leftBackEncoder;
-    private CANCoder m_rightBackEncoder;    
+    private CANCoder m_rightBackEncoder;
+        
     public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
     WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
     CANCoder rightFrontEncoder,
@@ -89,9 +90,7 @@ public class SwerveDrive extends SubsystemBase
         m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
-    
+
-        // PID
-        
     }
 
     // public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)

src/main/java/frc4388/utility/Gains.java

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Add your docs here. */
+public class Gains {
+    public double m_kP;
+    public double m_kI;
+    public double m_kD;
+    public double m_kF;
+    public int m_kIzone;
+    public double m_kPeakOutput;
+    public double m_kmaxOutput;
+    public double m_kminOutput;
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIzone The zone of the I value.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
+    {
+        m_kP = kP;
+        m_kI = kI;
+        m_kD = kD;
+        m_kF = kF;
+        m_kIzone = kIzone;
+        m_kPeakOutput = kPeakOutput;
+        m_kmaxOutput = m_kPeakOutput;
+        m_kminOutput = -m_kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIzone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput)
+    {
+        m_kP = kP;
+        m_kI = kI;
+        m_kD = kD;
+        m_kF = kF;
+        m_kIzone = kIzone;
+        m_kminOutput = kMinOutput;
+        m_kmaxOutput = kMaxOutput;
+        m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
+    }
+}