fixed controllers

hammerhead226 · Feb 19, 2024 · 82859c1 · 82859c1
1 parent aa35f22
commit 82859c1
Show file tree

Hide file tree

Showing 7 changed files with 64 additions and 88 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 12;
-  public static final String GIT_SHA = "65f9e459903602ab0738d226c636f60ff3e90899";
-  public static final String GIT_DATE = "2024-02-17 16:34:27 EST";
-  public static final String GIT_BRANCH = "CSnair7/2024RobotCode-Intake";
-  public static final String BUILD_DATE = "2024-02-18 21:07:06 EST";
-  public static final long BUILD_UNIX_TIME = 1708308426616L;
+  public static final int GIT_REVISION = 25;
+  public static final String GIT_SHA = "aa35f22a86da73486e46da4a99e9b72bfc757c42";
+  public static final String GIT_DATE = "2024-02-18 22:25:42 EST";
+  public static final String GIT_BRANCH = "main";
+  public static final String BUILD_DATE = "2024-02-18 22:30:19 EST";
+  public static final long BUILD_UNIX_TIME = 1708313419017L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -24,6 +24,15 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+  public static enum Mode {
+    REAL,
+    SIM,
+    REPLAY
+  }
+
+  public static final Mode currentMode = Mode.SIM;
+  public static final boolean tuningMode = true;
+  public static final String CANBUS = "CAN Bus 2";
 
   public static class SwerveConstants {
     public static final double MAX_LINEAR_SPEED = 5.56;
@@ -37,24 +46,12 @@ public static class SwerveConstants {
   public static class ModuleConstants {
     public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
 
-    public static final double DRIVE_STATOR_CURRENT_LIMIT = 40.0; 
-    public static final boolean DRIVE_STATOR_CURRENT_LIMIT_ENABLED = true; 
-    public static final double TURN_STATOR_CURRENT_LIMIT = 30.0; 
-    public static final boolean TURN_STATOR_CURRENT_LIMIT_ENABLED = true; 
-  }
-
-  public static final boolean tuningMode = true;
-
-  public static final String CANBUS = "CAN Bus 2";
-
-  public static enum Mode {
-    REAL,
-    SIM,
-    REPLAY
+    public static final double DRIVE_STATOR_CURRENT_LIMIT = 40.0;
+    public static final boolean DRIVE_STATOR_CURRENT_LIMIT_ENABLED = true;
+    public static final double TURN_STATOR_CURRENT_LIMIT = 30.0;
+    public static final boolean TURN_STATOR_CURRENT_LIMIT_ENABLED = true;
   }
 
-  public static final Mode currentMode = Mode.SIM;
-
   public static class IntakeConstants {
     public static final int ROLLER_CURRENT_LIMIT = 30;
     public static final boolean ROLLER_TALON_FX_CURRENT_LIMIT_ENABLED = true;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -32,7 +32,6 @@ public class Robot extends LoggedRobot {
   private Command autonomousCommand;
   private RobotContainer robotContainer;
 
-
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -88,7 +87,6 @@ public void robotInit() {
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
-
   }
 
   /** This function is called periodically during all modes. */

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -20,6 +20,7 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
@@ -29,17 +30,10 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.IntakeRollerIOSim;
 import frc.robot.subsystems.intake.IntakeRollerIOSparkFlex;
-
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -51,7 +45,6 @@ public class RobotContainer {
   // Subsystems
   private final Drive drive;
 
-
   private final CommandXboxController controller = new CommandXboxController(0);
   private final LoggedDashboardChooser<Command> autoChooser;
 
@@ -88,7 +81,7 @@ public RobotContainer() {
                 new ModuleIOSim(),
                 new ModuleIOSim());
         intake = new Intake(new IntakeRollerIOSim());
-        break; 
+        break;
       case SIM:
         // Sim robot, instantiate physics sim IO implementations
         drive =
@@ -124,7 +117,6 @@ public RobotContainer() {
             drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity));
     // Configure the button bindings
     configureButtonBindings();
-
   }
 
   /**
@@ -133,7 +125,6 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
-
   private void configureButtonBindings() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
@@ -152,11 +143,10 @@ private void configureButtonBindings() {
                     drive)
                 .ignoringDisable(true));
 
-
     // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
     // cancelling on release.
-    m_driverController.b().onTrue(new InstantCommand(() -> intake.runRollers(3)));
-    m_driverController.b().onFalse(new InstantCommand(() -> intake.stopRollers()));
+    controller.b().onTrue(new InstantCommand(() -> intake.runRollers(3)));
+    controller.b().onFalse(new InstantCommand(() -> intake.stopRollers()));
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -33,8 +33,6 @@
 import frc.robot.Constants;
 import frc.robot.util.LimelightHelpers;
 import frc.robot.util.LocalADStarAK;
-import frc.robot.util.LoggedTunableNumber;
-
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -50,9 +48,6 @@ public class Drive extends SubsystemBase {
 
   private final SwerveDrivePoseEstimator poseEstimator;
 
-  private static final LoggedTunableNumber translationkP = new LoggedTunableNumber("translationkP");
-  private static final LoggedTunableNumber rotationkP = new LoggedTunableNumber("rotationkP");
-
   public Drive(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
@@ -67,11 +62,24 @@ public Drive(
 
     lastGyroRotation = gyroInputs.yawPosition;
 
-    // init translationkP and rotationkP
-    translationkP.initDefault(0.5);
-    rotationkP.initDefault(0.05);
     // Configure AutoBuilder for PathPlanner
-    configureAutoBuilder(translationkP.get(), rotationkP.get());
+    AutoBuilder.configureHolonomic(
+        this::getPose,
+        this::setPose,
+        () -> kinematics.toChassisSpeeds(getModuleStates()),
+        this::runVelocity,
+        new HolonomicPathFollowerConfig(
+            new PIDConstants(0.5, 0, 0, 0),
+            new PIDConstants(0.05, 0, 0, 0),
+            Constants.SwerveConstants.MAX_LINEAR_SPEED,
+            Constants.SwerveConstants.DRIVE_BASE_RADIUS,
+            new ReplanningConfig()),
+        () -> {
+          var alliance = DriverStation.getAlliance();
+
+          return false;
+        },
+        this);
     // MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
     // this);
     Pathfinding.setPathfinder(new LocalADStarAK());
@@ -99,17 +107,13 @@ public Drive(
   }
 
   public void periodic() {
-    
+
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);
     for (var module : modules) {
       module.periodic();
     }
 
-    if (translationkP.hasChanged(hashCode()) || rotationkP.hasChanged(hashCode())) {
-      configureAutoBuilder(translationkP.get(), rotationkP.get());
-    }
-
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {
       for (var module : modules) {
@@ -134,27 +138,6 @@ public void periodic() {
     }
   }
 
-  // method that configures the autoBuilder every time the translation/rotation kPs change
-  public void configureAutoBuilder(double translationkP, double rotationkP) {
-    AutoBuilder.configureHolonomic(
-      this::getPose,
-      this::setPose,
-      () -> kinematics.toChassisSpeeds(getModuleStates()),
-      this::runVelocity,
-      new HolonomicPathFollowerConfig(
-          new PIDConstants(translationkP, 0, 0, 0),
-          new PIDConstants(rotationkP, 0, 0, 0),
-          Constants.SwerveConstants.MAX_LINEAR_SPEED,
-          Constants.SwerveConstants.DRIVE_BASE_RADIUS,
-          new ReplanningConfig()),
-      () -> {
-        var alliance = DriverStation.getAlliance();
-
-        return false;
-      },
-      this);
-  }
-
   public void updateOdometry() {
     poseEstimator.update(lastGyroRotation, getModulePositions());
 
@@ -185,7 +168,8 @@ public void runVelocity(ChassisSpeeds speeds) {
     // Calculate module setpoints
     ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
     SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, Constants.SwerveConstants.MAX_LINEAR_SPEED);
+    SwerveDriveKinematics.desaturateWheelSpeeds(
+        setpointStates, Constants.SwerveConstants.MAX_LINEAR_SPEED);
 
     // Send setpoints to modules
     SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
@@ -285,14 +269,18 @@ public double getMaxAngularSpeedRadPerSec() {
   /** Returns an array of module translations. */
   public static Translation2d[] getModuleTranslations() {
     return new Translation2d[] {
-      new Translation2d(Constants.SwerveConstants.TRACK_WIDTH_X / 2.0, 
-                        Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0),
-      new Translation2d(Constants.SwerveConstants.TRACK_WIDTH_X / 2.0, 
-                        -Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0),
-      new Translation2d(-Constants.SwerveConstants.TRACK_WIDTH_X / 2.0, 
-                        Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0),
-      new Translation2d(-Constants.SwerveConstants.TRACK_WIDTH_X / 2.0, 
-                        -Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0)
+      new Translation2d(
+          Constants.SwerveConstants.TRACK_WIDTH_X / 2.0,
+          Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0),
+      new Translation2d(
+          Constants.SwerveConstants.TRACK_WIDTH_X / 2.0,
+          -Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0),
+      new Translation2d(
+          -Constants.SwerveConstants.TRACK_WIDTH_X / 2.0,
+          Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0),
+      new Translation2d(
+          -Constants.SwerveConstants.TRACK_WIDTH_X / 2.0,
+          -Constants.SwerveConstants.TRACK_WIDTH_Y / 2.0)
     };
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import frc.robot.Constants;
 import frc.robot.util.LoggedTunableNumber;
-
 import org.littletonrobotics.junction.Logger;
 
 public class Module {

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -93,14 +93,18 @@ public ModuleIOTalonFX(int index) {
     }
 
     var driveConfig = new TalonFXConfiguration();
-    driveConfig.CurrentLimits.StatorCurrentLimit = Constants.ModuleConstants.DRIVE_STATOR_CURRENT_LIMIT;
-    driveConfig.CurrentLimits.StatorCurrentLimitEnable = Constants.ModuleConstants.DRIVE_STATOR_CURRENT_LIMIT_ENABLED;
+    driveConfig.CurrentLimits.StatorCurrentLimit =
+        Constants.ModuleConstants.DRIVE_STATOR_CURRENT_LIMIT;
+    driveConfig.CurrentLimits.StatorCurrentLimitEnable =
+        Constants.ModuleConstants.DRIVE_STATOR_CURRENT_LIMIT_ENABLED;
     driveTalon.getConfigurator().apply(driveConfig);
     setDriveBrakeMode(true);
 
     var turnConfig = new TalonFXConfiguration();
-    turnConfig.CurrentLimits.StatorCurrentLimit = Constants.ModuleConstants.TURN_STATOR_CURRENT_LIMIT;
-    turnConfig.CurrentLimits.StatorCurrentLimitEnable = Constants.ModuleConstants.TURN_STATOR_CURRENT_LIMIT_ENABLED;
+    turnConfig.CurrentLimits.StatorCurrentLimit =
+        Constants.ModuleConstants.TURN_STATOR_CURRENT_LIMIT;
+    turnConfig.CurrentLimits.StatorCurrentLimitEnable =
+        Constants.ModuleConstants.TURN_STATOR_CURRENT_LIMIT_ENABLED;
     turnTalon.getConfigurator().apply(turnConfig);
     setTurnBrakeMode(true);