From 7f71826af7ccdc9f83d66077900ffa8a0b8ebd6b Mon Sep 17 00:00:00 2001 From: William Coffer Date: Sun, 7 Jan 2024 15:04:12 -0800 Subject: [PATCH] drive straight for x meters command --- .../frc/robot/commands/DriveStraightCmd.java | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 src/main/java/frc/robot/commands/DriveStraightCmd.java diff --git a/src/main/java/frc/robot/commands/DriveStraightCmd.java b/src/main/java/frc/robot/commands/DriveStraightCmd.java new file mode 100644 index 00000000..06d291f5 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveStraightCmd.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 DriveStraightCmd extends CommandBase { + private final SwerveSubsystem m_swerveSubsystem; + private final Double rotationDegrees, meters; + private Double initialPosX, initialPosY; + + + public DriveStraightCmd(Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) { + m_swerveSubsystem = swerveSubsystem; + this.rotationDegrees = rotationDegrees; + this.meters = meters; + addRequirements(swerveSubsystem); + } + + @Override + 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)) + }; + m_swerveSubsystem.setModuleStates(module_states); + } + + @Override + public void end(boolean interrupted) { + m_swerveSubsystem.stopModules(); + } + + @Override + public boolean isFinished() { + return Math.sqrt(Math.pow(m_swerveSubsystem.getPose().getX() - initialPosX, 2) + Math.pow(m_swerveSubsystem.getPose().getY() - initialPosY, 2)) >= meters; + } +}