added clock drive command

Shenzhen-Robotics-Alliance · Dec 26, 2024 · bc25d75 · bc25d75
1 parent 41bf2f4
commit bc25d75
Show file tree

Hide file tree

Showing 8 changed files with 124 additions and 36 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -293,7 +293,9 @@ public void configureButtonBindings() {
                 : MapleJoystickDriveInput.leftHandedJoystick(driverXBox);
         final JoystickDrive joystickDrive =
                 new JoystickDrive(driveInput, () -> true, driverXBox.getHID()::getPOV, drive);
-        drive.setDefaultCommand(joystickDrive);
+        // drive.setDefaultCommand(joystickDrive);
+        drive.setDefaultCommand(
+                new ClockDrive(drive, driveInput, () -> -driverXBox.getRightY(), () -> -driverXBox.getRightX()));
 
         /* lock chassis with x-formation */
         driverXBox.x().whileTrue(drive.lockChassisWithXFormation());

diff --git a/src/main/java/frc/robot/commands/drive/ClockDrive.java b/src/main/java/frc/robot/commands/drive/ClockDrive.java
@@ -0,0 +1,67 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
+import frc.robot.utils.ChassisHeadingController;
+import frc.robot.utils.MapleJoystickDriveInput;
+import java.util.function.DoubleSupplier;
+import org.ironmaple.utils.FieldMirroringUtils;
+
+public class ClockDrive extends Command {
+    public static final double ROTATION_AXIS_THRESHOLD = 0.5;
+
+    private final HolonomicDriveSubsystem driveSubsystem;
+    private final MapleJoystickDriveInput input;
+    private final DoubleSupplier rotationXSupplier, rotationYSupplier;
+
+    private ChassisSpeeds currentPilotInputSpeeds;
+    private Rotation2d currentDesiredFacing;
+
+    public ClockDrive(
+            HolonomicDriveSubsystem driveSubsystem,
+            MapleJoystickDriveInput input,
+            DoubleSupplier rotationXSupplier,
+            DoubleSupplier rotationYSupplier) {
+        this.driveSubsystem = driveSubsystem;
+        this.input = input;
+        this.rotationXSupplier = rotationXSupplier;
+        this.rotationYSupplier = rotationYSupplier;
+
+        super.addRequirements(driveSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        currentPilotInputSpeeds = new ChassisSpeeds();
+        currentDesiredFacing = driveSubsystem.getFacing();
+    }
+
+    @Override
+    public void execute() {
+        final ChassisSpeeds newestPilotInputSpeed = input.getJoystickChassisSpeeds(
+                driveSubsystem.getChassisMaxLinearVelocityMetersPerSec(),
+                driveSubsystem.getChassisMaxAngularVelocity());
+        currentPilotInputSpeeds = driveSubsystem.constrainAcceleration(
+                currentPilotInputSpeeds, newestPilotInputSpeed, Robot.defaultPeriodSecs);
+
+        Translation2d headingVector =
+                new Translation2d(rotationXSupplier.getAsDouble(), rotationYSupplier.getAsDouble());
+        if (headingVector.getNorm() > ROTATION_AXIS_THRESHOLD)
+            this.currentDesiredFacing =
+                    headingVector.getAngle().plus(FieldMirroringUtils.getCurrentAllianceDriverStationFacing());
+
+        ChassisHeadingController.getInstance()
+                .setHeadingRequest(new ChassisHeadingController.FaceToRotationRequest(this.currentDesiredFacing));
+
+        driveSubsystem.runDriverStationCentricChassisSpeeds(currentPilotInputSpeeds, true);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
+    }
+}
diff --git a/src/main/java/frc/robot/commands/drive/FollowPathFaceToTarget.java b/src/main/java/frc/robot/commands/drive/FollowPathFaceToTarget.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.subsystems.drive.SwerveDrive;
 import frc.robot.utils.ChassisHeadingController;
 import frc.robot.utils.MapleShooterOptimization;
 import java.util.function.Supplier;
@@ -17,21 +16,22 @@ public static Command followPathFacetToTarget(
             double offSetSeconds,
             Supplier<Translation2d> targetPositionSupplier,
             MapleShooterOptimization shooterOptimization) {
-        final Runnable requestFaceToTarget = () -> SwerveDrive.swerveHeadingController.setHeadingRequest(
-                new ChassisHeadingController.FaceToTargetRequest(targetPositionSupplier, shooterOptimization));
-        final Runnable requestNull =
-                () -> SwerveDrive.swerveHeadingController.setHeadingRequest(new ChassisHeadingController.NullRequest());
+        final Runnable requestFaceToTarget = () -> ChassisHeadingController.getInstance()
+                .setHeadingRequest(
+                        new ChassisHeadingController.FaceToTargetRequest(targetPositionSupplier, shooterOptimization));
+        final Runnable requestNull = () ->
+                ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
         return AutoBuilder.followPath(path)
                 .deadlineFor(Commands.waitSeconds(offSetSeconds).andThen(requestFaceToTarget))
                 .finallyDo(requestNull);
     }
 
     public static Command followPathFacetToTarget(
             PathPlannerPath path, double offSetSeconds, Supplier<Rotation2d> rotationTargetOverride) {
-        final Runnable requestFaceToRotation = () -> SwerveDrive.swerveHeadingController.setHeadingRequest(
-                new ChassisHeadingController.FaceToRotationRequest(rotationTargetOverride.get()));
-        final Runnable requestNull =
-                () -> SwerveDrive.swerveHeadingController.setHeadingRequest(new ChassisHeadingController.NullRequest());
+        final Runnable requestFaceToRotation = () -> ChassisHeadingController.getInstance()
+                .setHeadingRequest(new ChassisHeadingController.FaceToRotationRequest(rotationTargetOverride.get()));
+        final Runnable requestNull = () ->
+                ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
         return AutoBuilder.followPath(path)
                 .deadlineFor(Commands.waitSeconds(offSetSeconds).andThen(requestFaceToRotation))
                 .finallyDo(requestNull);

diff --git a/src/main/java/frc/robot/commands/drive/JoystickDrive.java b/src/main/java/frc/robot/commands/drive/JoystickDrive.java
@@ -11,15 +11,14 @@
 import frc.robot.Robot;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
-import frc.robot.subsystems.drive.SwerveDrive;
 import frc.robot.utils.ChassisHeadingController;
 import frc.robot.utils.MapleJoystickDriveInput;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 public class JoystickDrive extends Command {
-    protected MapleJoystickDriveInput input;
+    protected final MapleJoystickDriveInput input;
     private final BooleanSupplier useDriverStationCentricSwitch;
     private final Supplier<Integer> povButtonSupplier;
     private final HolonomicDriveSubsystem driveSubsystem;
@@ -47,7 +46,7 @@ public JoystickDrive(
 
         super.addRequirements(driveSubsystem);
         resetSensitivity();
-        SwerveDrive.swerveHeadingController.setHeadingRequest(new ChassisHeadingController.NullRequest());
+        ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
     }
 
     @Override
@@ -60,8 +59,6 @@ public void initialize() {
 
     @Override
     public void execute() {
-        if (input == null) return;
-
         final ChassisSpeeds newestPilotInputSpeed = input.getJoystickChassisSpeeds(
                 driveSubsystem.getChassisMaxLinearVelocityMetersPerSec() * translationalSensitivity,
                 driveSubsystem.getChassisMaxAngularVelocity() * rotationalSensitivity);
@@ -76,10 +73,11 @@ public void execute() {
 
         if (previousRotationalInputTimer.hasElapsed(
                 TIME_ACTIVATE_ROTATION_MAINTENANCE_AFTER_NO_ROTATIONAL_INPUT_SECONDS))
-            SwerveDrive.swerveHeadingController.setHeadingRequest(
-                    new ChassisHeadingController.FaceToRotationRequest(currentRotationMaintenanceSetpoint));
+            ChassisHeadingController.getInstance()
+                    .setHeadingRequest(
+                            new ChassisHeadingController.FaceToRotationRequest(currentRotationMaintenanceSetpoint));
         else {
-            SwerveDrive.swerveHeadingController.setHeadingRequest(new ChassisHeadingController.NullRequest());
+            ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
             currentRotationMaintenanceSetpoint = driveSubsystem.getFacing();
         }
 
@@ -108,4 +106,9 @@ public void setSensitivity(double translationalSensitivity, double rotationalSen
     public void resetSensitivity() {
         setSensitivity(DEFAULT_TRANSLATIONAL_SENSITIVITY, DEFAULT_ROTATIONAL_SENSITIVITY);
     }
+
+    @Override
+    public void end(boolean interrupted) {
+        ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
+    }
 }
diff --git a/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java b/src/main/java/frc/robot/commands/drive/JoystickDriveAndAimAtTarget.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
-import frc.robot.subsystems.drive.SwerveDrive;
 import frc.robot.utils.ChassisHeadingController;
 import frc.robot.utils.MapleJoystickDriveInput;
 import frc.robot.utils.MapleShooterOptimization;
@@ -44,8 +43,9 @@ public JoystickDriveAndAimAtTarget(
 
     @Override
     public void initialize() {
-        SwerveDrive.swerveHeadingController.setHeadingRequest(
-                new ChassisHeadingController.FaceToTargetRequest(targetPositionSupplier, shooterOptimization));
+        ChassisHeadingController.getInstance()
+                .setHeadingRequest(
+                        new ChassisHeadingController.FaceToTargetRequest(targetPositionSupplier, shooterOptimization));
     }
 
     @Override
@@ -57,11 +57,11 @@ public void execute() {
     }
 
     public boolean chassisRotationInPosition() {
-        return SwerveDrive.swerveHeadingController.atSetPoint();
+        return ChassisHeadingController.getInstance().atSetPoint();
     }
 
     @Override
     public void end(boolean interrupted) {
-        SwerveDrive.swerveHeadingController.setHeadingRequest(new ChassisHeadingController.NullRequest());
+        ChassisHeadingController.getInstance().setHeadingRequest(new ChassisHeadingController.NullRequest());
     }
 }
diff --git a/src/main/java/frc/robot/commands/drive/PathFindToPose.java b/src/main/java/frc/robot/commands/drive/PathFindToPose.java
@@ -5,7 +5,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.commands.CommandOnFly;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
-import frc.robot.subsystems.drive.SwerveDrive;
 import frc.robot.utils.ChassisHeadingController;
 import java.util.function.Supplier;
 
@@ -17,7 +16,9 @@ public PathFindToPose(
             double goalEndVelocity) {
         super(() -> AutoBuilder.pathfindToPose(
                         targetPose.get(), driveSubsystem.getChassisConstrains(speedMultiplier), goalEndVelocity)
-                .beforeStarting(Commands.runOnce(() -> SwerveDrive.swerveHeadingController.setHeadingRequest(
-                        new ChassisHeadingController.NullRequest()))));
+                .beforeStarting(Commands.runOnce(() -> ChassisHeadingController.getInstance()
+                        .setHeadingRequest(new ChassisHeadingController.NullRequest())))
+                .finallyDo(() -> ChassisHeadingController.getInstance()
+                        .setHeadingRequest(new ChassisHeadingController.NullRequest())));
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java
@@ -18,12 +18,11 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.constants.DriveControlLoops;
+import frc.robot.Robot;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.IO.*;
@@ -54,12 +53,6 @@ public enum DriveType {
     private final OdometryThread odometryThread;
     private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.ERROR),
             visionNoResultAlert = new Alert("Vision No Result", Alert.AlertType.INFO);
-    public static final ChassisHeadingController swerveHeadingController = new ChassisHeadingController(
-            new TrapezoidProfile.Constraints(
-                    CHASSIS_MAX_ANGULAR_VELOCITY.in(RadiansPerSecond),
-                    CHASSIS_MAX_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)),
-            DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP,
-            new Rotation2d());
 
     public SwerveDrive(
             DriveType type,
@@ -199,10 +192,12 @@ private void updateRobotFacingWithOdometry(SwerveModulePosition[] modulesDelta)
     @Override
     public void runRawChassisSpeeds(ChassisSpeeds speeds) {
         OptionalDouble angularVelocityOverride =
-                swerveHeadingController.calculate(getMeasuredChassisSpeedsFieldRelative(), getPose());
-        if (angularVelocityOverride.isPresent())
+                ChassisHeadingController.getInstance().calculate(getMeasuredChassisSpeedsFieldRelative(), getPose());
+        if (angularVelocityOverride.isPresent()) {
             speeds = new ChassisSpeeds(
                     speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, angularVelocityOverride.getAsDouble());
+            speeds = ChassisSpeeds.discretize(speeds, Robot.defaultPeriodSecs);
+        }
 
         SwerveModuleState[] setPointStates = DRIVE_KINEMATICS.toSwerveModuleStates(speeds);
         SwerveDriveKinematics.desaturateWheelSpeeds(setPointStates, CHASSIS_MAX_VELOCITY);

diff --git a/src/main/java/frc/robot/utils/ChassisHeadingController.java b/src/main/java/frc/robot/utils/ChassisHeadingController.java
@@ -1,11 +1,17 @@
 package frc.robot.utils;
 
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static frc.robot.constants.DriveTrainConstants.CHASSIS_MAX_ANGULAR_ACCELERATION;
+import static frc.robot.constants.DriveTrainConstants.CHASSIS_MAX_ANGULAR_VELOCITY;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc.robot.Robot;
+import frc.robot.constants.DriveControlLoops;
 import frc.robot.utils.CustomPIDs.MaplePIDController;
 import java.util.OptionalDouble;
 import java.util.function.Supplier;
@@ -238,4 +244,18 @@ private void log(Pose2d robotPose, Rotation2d requestedRotation) {
     public boolean atSetPoint() {
         return atSetPoint;
     }
+
+    private static ChassisHeadingController instance = null;
+
+    public static ChassisHeadingController getInstance() {
+        if (instance == null)
+            instance = new ChassisHeadingController(
+                    new TrapezoidProfile.Constraints(
+                            CHASSIS_MAX_ANGULAR_VELOCITY.in(RadiansPerSecond),
+                            CHASSIS_MAX_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)),
+                    DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP,
+                    new Rotation2d());
+
+        return instance;
+    }
 }