From c9b27fca0562322ddb5cbf2e4fe70468f19337d7 Mon Sep 17 00:00:00 2001 From: gaming <48131223+TheGamer1002@users.noreply.github.com> Date: Mon, 12 Feb 2024 16:34:27 -0500 Subject: [PATCH] feat(swerve): add command, add link to subsystem --- .../java/frc/robot/commands/DriveCommand.java | 106 ++++++++++++++++-- .../frc/robot/subsystems/SwerveSubsystem.java | 1 + 2 files changed, 97 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 7a03d48..706a5a4 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -2,26 +2,112 @@ import edu.wpi.first.wpilibj2.command.Command; +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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.util.Constants; +import frc.robot.subsystems.SwerveSubsystem; +import java.util.List; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + public class DriveCommand extends Command { - public DriveCommand() { - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(); + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingHorizontal, headingVertical; + private boolean initRotation = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is + * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian + * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot + * will rotate to. + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 + * to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 + * to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's heading angle. In the + * robot coordinate system, this is along the same axis as vY. Should range from -1 to 1 with + * no deadband. Positive is towards the left wall when looking through the driver station + * glass. + * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's heading angle. In the + * robot coordinate system, this is along the same axis as vX. Should range from -1 to 1 + * with no deadband. Positive is away from the alliance wall. + */ + public DriveCommand(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal, + DoubleSupplier headingVertical) + { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingHorizontal = headingHorizontal; + this.headingVertical = headingVertical; + + addRequirements(swerve); } @Override - public void initialize() {} + public void initialize() + { + initRotation = true; + } + // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() + { + + // Get the desired chassis speeds based on a 2 joystick module. + ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), + headingHorizontal.getAsDouble(), + headingVertical.getAsDouble()); + + // Prevent Movement After Auto + if (initRotation) + { + if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) + { + // Get the curretHeading + Rotation2d firstLoopHeading = swerve.getHeading(); + // Set the Current Heading to the desired Heading + desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + } + //Dont Init Rotation Again + initRotation = false; + } + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), + Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + + } + + // Called once the command ends or is interrupted. @Override - public boolean isFinished() { - // TODO: Make this return true when this Command no longer needs to run execute() - return false; + public void end(boolean interrupted) + { } + // Returns true when the command should end. @Override - public void end(boolean interrupted) {} + public boolean isFinished() + { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index a0e4055..7c31e69 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -138,6 +138,7 @@ public SwerveDriveKinematics getKinematics() { return swerveDrive.kinematics; } + // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html /** * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when * calling this method. However, if either gyro angle or module position is reset, this must be