Refractor BaseDrive

src/main/java/frc/robot/RobotContainer.java

@@ -23,7 +23,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import frc.robot.commands.DriveCommands;
 import frc.robot.commands.ResetEncoderCommand;
 import frc.robot.constants.Constants;
 import frc.robot.constants.DriveConstants;
@@ -52,6 +51,7 @@
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import frc.robot.commands.*;
 
 public class RobotContainer {
   // Subsystems
@@ -204,11 +204,11 @@ void loadCommands() {
     operatorInput.resetEncoder.onTrue(new ResetEncoderCommand(elevatorSubsystem));
 
     operatorInput.movementDesired.whileTrue(
-        DriveCommands.BaseDriveCommand(
-            drive,
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> -controller.getRightX()));
+            new BaseDriveCommand(
+                drive,
+                () -> -controller.getLeftY(),
+                () -> -controller.getLeftX(),
+                () -> -controller.getRightX()));
   } // TODO FIX COMMAND THIS WILL BREAK DO NOT RUN IT
 
   /**

src/main/java/frc/robot/commands/BaseDriveCommand.java

@@ -0,0 +1,85 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
+
+import java.util.function.DoubleSupplier;
+
+public class BaseDriveCommand extends Command {
+    private final DriveSubsystem drive;
+    private final DoubleSupplier xSupplier;
+    private final DoubleSupplier ySupplier;
+    private final DoubleSupplier omegaSupplier;
+
+    public BaseDriveCommand(DriveSubsystem drive,
+                            DoubleSupplier xSupplier,
+                            DoubleSupplier ySupplier,
+                            DoubleSupplier omegaSupplier) {
+        this.drive = drive;
+        this.xSupplier = xSupplier;
+        this.ySupplier = ySupplier;
+        this.omegaSupplier = omegaSupplier;
+    }
+
+    @Override
+    public void execute() {
+        // Apply deadband
+        double DEADBAND = 0.1;
+        double linearMagnitude =
+                MathUtil.applyDeadband(
+                        Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
+        Rotation2d linearDirection =
+                new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+        double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);
+
+        // Square values
+        linearMagnitude = linearMagnitude * linearMagnitude;
+        omega = Math.copySign(omega * omega, omega);
+
+        // Calcaulate new linear velocity
+        Translation2d linearVelocity =
+                new Pose2d(new Translation2d(), linearDirection)
+                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+                        .getTranslation();
+
+        // Convert to field relative speeds & send command
+        boolean isFlipped =
+                DriverStation.getAlliance().isPresent()
+                        && DriverStation.getAlliance().get() == Alliance.Red;
+        drive.runVelocityPP(
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                        linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+                        linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+                        omega * drive.getMaxAngularSpeedRadPerSec(),
+                        isFlipped
+                                ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                                : drive.getRotation()));
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drive.stop();
+    }
+}

src/main/java/frc/robot/subsystems/DriveSubsystem/DriveSubsystem.java

@@ -19,9 +19,7 @@
 import choreo.trajectory.SwerveSample;
 import com.ctre.phoenix6.CANBus;
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.pathplanner.lib.util.DriveFeedforwards;
@@ -43,7 +41,6 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -237,7 +234,7 @@ public void runVelocity(ChassisSpeeds speeds) {
   }
 
   // use pathplanners's / team 254's setpoint generator
-  public void runRobotRelativePathPlanner(ChassisSpeeds speeds) {
+  public void runVelocityPP(ChassisSpeeds speeds) {
     // Note: it is important to not discretize speeds before or after
     // using the setpoint generator, as it will discretize them for you
     previousSetpoint = setpointGenerator.generateSetpoint(
@@ -258,7 +255,7 @@ public void runRobotRelativePathPlanner(ChassisSpeeds speeds) {
 
   //no feedback yet!!!! TODO
   public void followTrajectorySample(SwerveSample sample) {
-    runRobotRelativePathPlanner(sample.getChassisSpeeds());
+    runVelocityPP(sample.getChassisSpeeds());
   }
 
   /** Runs the drive in a straight line with the specified drive output. */