WIP FF CODE

team422 · Nov 4, 2023 · 351ffdb · 351ffdb
1 parent 07af7e7
commit 351ffdb
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -2,6 +2,8 @@
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -14,10 +16,12 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.ModuleConstants;
 import frc.robot.subsystems.drive.accelerometer.AccelerometerIO;
 import frc.robot.subsystems.drive.accelerometer.AccelerometerInputsAutoLogged;
 import frc.robot.subsystems.drive.gyro.GyroIO;
 import frc.robot.subsystems.drive.gyro.GyroInputsAutoLogged;
+import frc.robot.util.TunableNumber;
 
 public class Drive extends SubsystemBase {
 
@@ -37,6 +41,26 @@ public class Drive extends SubsystemBase {
     private int m_oneWheelIndex = 2;
     private final double[] m_lockAngles = new double[] { 45, 315, 45, 315 };
 
+    private final PIDController m_drivePIDController;
+    private final PIDController m_turningPIDController;
+    private final SimpleMotorFeedforward m_driveFFController;
+
+    public static class ModuleConstants {
+        public static final double kDriveConversionFactor = 1/20.462;
+        public static final double kTurnPositionConversionFactor = 21.428;
+        public static final TunableNumber kDriveP = Constants.ModuleConstants.kDriveP;
+        public static final TunableNumber kDriveI = Constants.ModuleConstants.kDriveI;
+        public static final TunableNumber kDriveD = Constants.ModuleConstants.kDriveD;
+        public static final TunableNumber kTurningP = Constants.ModuleConstants.kTurningP;
+        public static final TunableNumber kTurningI = Constants.ModuleConstants.kTurningI;
+        public static final TunableNumber kTurningD = Constants.ModuleConstants.kTurningD;   
+
+        public static final TunableNumber kDriveS = Constants.ModuleConstants.kDriveS;
+        public static final TunableNumber kDriveV = Constants.ModuleConstants.kDriveV;
+        public static final TunableNumber kDriveA = Constants.ModuleConstants.kDriveA;
+
+      }
+
     /** Creates a new Drive. */
     public Drive(GyroIO gyro, AccelerometerIO accel, Pose2d startPose, SwerveModuleIO... modules) {
         m_modules = modules;
@@ -58,6 +82,12 @@ public Drive(GyroIO gyro, AccelerometerIO accel, Pose2d startPose, SwerveModuleI
 
         m_accel = accel;
         m_accelInputs = new AccelerometerInputsAutoLogged();
+
+        m_drivePIDController = new PIDController(ModuleConstants.kDriveP.get(), ModuleConstants.kDriveI.get(), ModuleConstants.kDriveD.get());
+        m_turningPIDController = new PIDController(ModuleConstants.kTurningP.get(), ModuleConstants.kTurningI.get(), ModuleConstants.kTurningD.get());
+        m_driveFFController = new SimpleMotorFeedforward(ModuleConstants.kDriveS.get(), ModuleConstants.kDriveV.get(), ModuleConstants.kDriveA.get());
+
+
     }
 
     public SwerveModulePosition[] getSwerveModulePositions() {
@@ -92,6 +122,7 @@ public void periodic() {
         // Update Swerve Module Inputs/Logs
         for (int i = 0; i < m_modules.length; i++) {
             m_modules[i].updateInputs(m_inputs[i]);
+            // double drivePidVoltage = m_drivePIDController.calculate(m_inputs[i]., m_inputs[i].);
             Logger.getInstance().processInputs("Swerve Module " + i, m_inputs[i]);
         }
 

diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModuleIOMK4iSparkMax.java b/src/main/java/frc/robot/subsystems/drive/SwerveModuleIOMK4iSparkMax.java
@@ -183,15 +183,18 @@ public Rotation2d getAbsoluteRotation() {
 
 //   @Override
   public void setVoltage(double voltageDrive, double voltageTurn) {
-    m_driveController.setReference(voltageDrive, ControlType.kVoltage);
-    m_turningController.setReference(voltageTurn, ControlType.kVoltage);
+    // m_driveController.setReference(voltageDrive, ControlType.kVoltage);
+    // m_turningController.setReference(voltageTurn, ControlType.kVoltage);
+    m_driveMotor.setVoltage(voltageDrive);
+    m_turningMotor.setVoltage(voltageTurn);
   }
 
   public void setVoltageDriveOnly(double voltageDrive, SwerveModulePosition state) {
-    m_driveController.setReference(voltageDrive, ControlType.kVoltage);
-    m_turningController.setReference(
-        state.angle.getDegrees(),
-        ControlType.kPosition);
+    // m_driveController.setReference(voltageDrive, ControlType.kVoltage);
+    // m_turningController.setReference(
+    //     state.angle.getDegrees(),
+    //     ControlType.kPosition);
+    m_driveMotor.setVoltage(voltageDrive);
   }
 
   /**
@@ -203,13 +206,15 @@ public void setVoltageDriveOnly(double voltageDrive, SwerveModulePosition state)
   @Override
   public void setDesiredState(SwerveModuleState state) {
     double driveOutput = state.speedMetersPerSecond;
-    m_turningController.setReference(
-        state.angle.getDegrees(),
-        ControlType.kPosition);
+    // m_turningController.setReference(
+    //     state.angle.getDegrees(),
+    //     ControlType.kPosition);
+
 
     adjustedSpeed = driveOutput;
-    m_driveController.setReference(driveOutput, ControlType.kVelocity, 0,
-        adjustedSpeed);
+    // m_driveController.setReference(driveOutput, ControlType.kVelocity, 0,
+    //     adjustedSpeed);
+    m_driveMotor.setVoltage(driveOutput);
     if (Constants.tuningMode) {
       // m_turningController.setP(ModuleConstants.kTurningP.get());
       // m_turningController.setI(ModuleConstants.kTurningI.get());