diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e25832f..29bb1fe 100644 --- 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 new file mode 100644 index 0000000..5153ac2 --- /dev/null +++ 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 index a2ea5de..3812de3 100644 --- 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,10 +16,11 @@ public static Command followPathFacetToTarget( double offSetSeconds, Supplier 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); @@ -28,10 +28,10 @@ public static Command followPathFacetToTarget( public static Command followPathFacetToTarget( PathPlannerPath path, double offSetSeconds, Supplier 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 index 8f54e95..fd7d5d9 100644 --- a/src/main/java/frc/robot/commands/drive/JoystickDrive.java +++ b/src/main/java/frc/robot/commands/drive/JoystickDrive.java @@ -11,7 +11,6 @@ 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; @@ -19,7 +18,7 @@ import org.littletonrobotics.junction.Logger; public class JoystickDrive extends Command { - protected MapleJoystickDriveInput input; + protected final MapleJoystickDriveInput input; private final BooleanSupplier useDriverStationCentricSwitch; private final Supplier 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 index 7a933aa..5fba106 100644 --- 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 index ef4dfe6..a83aeab 100644 --- 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 index d8bfc52..f908ac7 100644 --- 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 index 5631f43..fa2cf9a 100644 --- 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; + } }