From b473f2cee5fcee59265285476deffcfb45dd874d Mon Sep 17 00:00:00 2001 From: William Coffer Date: Sun, 7 Jan 2024 15:12:48 -0800 Subject: [PATCH] rotate x degrees comand --- .../frc/robot/commands/DriveStraightCmd.java | 8 ++-- .../frc/robot/commands/RotateDegreesCmd.java | 43 +++++++++++++++++++ 2 files changed, 47 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/RotateDegreesCmd.java diff --git a/src/main/java/frc/robot/commands/DriveStraightCmd.java b/src/main/java/frc/robot/commands/DriveStraightCmd.java index 06d291f5..fd96ce7a 100644 --- a/src/main/java/frc/robot/commands/DriveStraightCmd.java +++ b/src/main/java/frc/robot/commands/DriveStraightCmd.java @@ -23,10 +23,10 @@ public void execute() { initialPosX = m_swerveSubsystem.getPose().getX(); initialPosY = m_swerveSubsystem.getPose().getY(); SwerveModuleState[] module_states = { - new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)), - new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)), - new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)), - new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)) + new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)), + new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)), + new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)), + new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)) }; m_swerveSubsystem.setModuleStates(module_states); } diff --git a/src/main/java/frc/robot/commands/RotateDegreesCmd.java b/src/main/java/frc/robot/commands/RotateDegreesCmd.java new file mode 100644 index 00000000..f0c12c40 --- /dev/null +++ b/src/main/java/frc/robot/commands/RotateDegreesCmd.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.SwerveSubsystem; + +public class RotateDegreesCmd extends CommandBase { + private final SwerveSubsystem m_swerveSubsystem; + private final Double rotationDegrees; + private final Boolean rotateRight; + private Double initialRotation; + + + public RotateDegreesCmd(Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) { + m_swerveSubsystem = swerveSubsystem; + this.rotationDegrees = rotationDegrees; + this.rotateRight = rotateRight; + addRequirements(swerveSubsystem); + } + + @Override + public void execute() { + initialRotation = m_swerveSubsystem.getPose().getRotation().getDegrees(); + SwerveModuleState[] module_states = { + new SwerveModuleState(1.0, Rotation2d.fromDegrees(135 * (rotateRight ? 1 : -1))), + new SwerveModuleState(1.0, Rotation2d.fromDegrees(45 * (rotateRight ? 1 : -1))), + new SwerveModuleState(1.0, Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))), + new SwerveModuleState(1.0, Rotation2d.fromDegrees(-45 * (rotateRight ? 1 : -1))) + }; + m_swerveSubsystem.setModuleStates(module_states); + } + + @Override + public void end(boolean interrupted) { + m_swerveSubsystem.stopModules(); + } + + @Override + public boolean isFinished() { + return Math.abs(initialRotation - m_swerveSubsystem.getPose().getRotation().getDegrees()) >= rotationDegrees; + } +}