diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 56547a9..92350b9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f1f9890..2c728c1 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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) diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java new file mode 100644 index 0000000..e3e136f --- /dev/null +++ b/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); + } +}