Merge branch 'main' of https://github.com/Team334/R2024

Team334 · Jan 25, 2024 · b01c659 · b01c659
2 parents 1c7f324 + 86e253b
commit b01c659
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -7,6 +7,7 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -76,7 +77,10 @@ public static class Physical {
   }
 
   public static class FeedForward {
-    public static final double ELEVATOR_KG = 0.0; // TODO: Find Kg constant.
+    public static final double ELEVATOR_KG = 0.0; // TODO: Find Kg constants
+
+    public static final double MODULE_DRIVE_KS = 0.0;
+    public static final double MODULE_DRIVE_KV = 0.0;
   }
 
   public static class PID {

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -14,6 +14,8 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -22,12 +24,14 @@
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 import frc.robot.utils.BNO055;
 import frc.robot.utils.SwerveModule;
+import frc.robot.utils.UtilFuncs;
+
 import java.util.Optional;
 
-import javax.swing.text.html.Option;
 
 /**
  * @author Peter Gutkovich
@@ -101,6 +105,20 @@ public class SwerveDriveSubsystem extends SubsystemBase {
 
   StructArrayPublisher<SwerveModuleState> publisher = NetworkTableInstance.getDefault().getStructArrayTopic("MyStates", SwerveModuleState.struct).publish();
 
+  // private final SysIdRoutine _sysID = new SysIdRoutine(
+  //   new SysIdRoutine.Config(),
+  //   new SysIdRoutine.Mechanism(
+  //     (volts) -> {
+  //       driveTest(UtilFuncs.FromVolts(volts.in(Units.Volts)));
+  //     },
+  //     (log) -> {
+  //       new Measure()
+  //       // log.motor("Front Left").linearVelocity(_frontLeft.getDriveVelocity());
+  //     },
+  //     this
+  //   )
+  // );
+
   // Pose Estimator -> Has built in odometry and uses supplied vision measurements
   private final SwerveDrivePoseEstimator _estimator = new SwerveDrivePoseEstimator(
     Constants.Physical.SWERVE_KINEMATICS,

diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java
@@ -7,6 +7,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -24,6 +25,11 @@ public class SwerveModule {
   private final PIDController _driveController;
   private final PIDController _rotationController;
 
+  private final SimpleMotorFeedforward _driveFeedforward = new SimpleMotorFeedforward(
+    Constants.FeedForward.MODULE_DRIVE_KS,
+    Constants.FeedForward.MODULE_DRIVE_KV
+  );
+
   private final CANcoder _encoder;
 
   private final String _name;
@@ -153,7 +159,8 @@ public void setState(SwerveModuleState state) {
       0.150
     );
 
-    double drive_feedforward = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED);
+    // double drive_feedforward = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED);
+    double drive_feedforward = UtilFuncs.FromVolts(_driveFeedforward.calculate(speed));
     double drive_pid = _driveController.calculate(getDriveVelocity(), speed);
 
     rotate(rotation_pid);

diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java
@@ -3,6 +3,8 @@
 
 package frc.robot.utils;
 
+import edu.wpi.first.wpilibj.RobotController;
+
 /** Any utility functions are here. */
 public final class UtilFuncs {
   /**
@@ -19,4 +21,14 @@ public static double ApplyDeadband(double val, double deadband) {
 
     return 0;
   }
+
+  /**
+   * Control a motor controller with voltage by converting a voltage output into percent output with a scale factor.
+   * 
+   * @param volts The voltage output.
+   * @return The percent output to set the controller to.
+   */
+  public static double FromVolts(double volts) {
+    return volts / 12.0;
+  }
 }