diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index b13796e1..65d39bd8 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,5 +1,6 @@ package org.team1540.robot2024; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -232,6 +233,16 @@ public static class Tramp { public static final int MOTOR_ID = -1; //TODO: Configure this later } + public static class Targeting { + // TODO: tune these + public static final double ROT_KP = 1.18; + public static final double ROT_KI = 0.0; + public static final double ROT_KD = 0.0; + + public static final Pose2d SPEAKER_POSE = + new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d()); + } + public static boolean isTuningMode() { return tuningMode && !DriverStation.isFMSAttached(); } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6f7840a7..1840f5df 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -9,15 +9,14 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.team1540.robot2024.Constants.Elevator.ElevatorState; import org.team1540.robot2024.commands.FeedForwardCharacterization; +import org.team1540.robot2024.commands.DriveWithSpeakerTargetingCommand; import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.commands.elevator.ElevatorManualCommand; import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand; import org.team1540.robot2024.commands.indexer.IntakeCommand; import org.team1540.robot2024.commands.shooter.ShootSequence; -import org.team1540.robot2024.commands.shooter.TuneShooterCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.elevator.Elevator; import org.team1540.robot2024.subsystems.elevator.ElevatorIO; @@ -176,6 +175,7 @@ private void configureButtonBindings() { indexer.setDefaultCommand(new IntakeCommand(indexer, tramp)); driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); + driver.y().toggleOnTrue(new DriveWithSpeakerTargetingCommand(drivetrain, driver)); driver.b().onTrue( Commands.runOnce( () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())), diff --git a/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java new file mode 100644 index 00000000..0bf10fe7 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java @@ -0,0 +1,72 @@ +package org.team1540.robot2024.commands; + +import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.subsystems.drive.Drivetrain; +import org.team1540.robot2024.util.LoggedTunableNumber; + +import static org.team1540.robot2024.Constants.Targeting.*; + +public class DriveWithSpeakerTargetingCommand extends Command { + private final Drivetrain drivetrain; + private final CommandXboxController controller; + + private final SlewRateLimiter xLimiter = new SlewRateLimiter(2); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(2); + private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD); + + private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD); + + private boolean isFlipped; + private Pose2d speakerPose; + + public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) { + this.drivetrain = drivetrain; + this.controller = controller; + rotController.enableContinuousInput(-Math.PI, Math.PI); + addRequirements(drivetrain); + } + + @Override + public void initialize() { + rotController.reset(); + + isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE; + } + + @Override + public void execute() { + if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) { + rotController.setPID(kP.get(), kI.get(), kD.get()); + } + + Rotation2d targetRot = + drivetrain.getPose().minus(speakerPose).getTranslation().getAngle() + .rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180)); + Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot)); + Logger.recordOutput("Targeting/speakerPose", speakerPose); + + double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); + double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); + double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians()); + drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } +} diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java index de3995cc..60f2b29e 100644 --- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java @@ -2,11 +2,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -30,9 +25,6 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle @Override public void initialize() { - xLimiter.reset(0); - yLimiter.reset(0); - rotLimiter.reset(0); isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; @@ -40,31 +32,11 @@ public void initialize() { @Override public void execute() { - double xPercent = xLimiter.calculate(-controller.getLeftY()); - double yPercent = yLimiter.calculate(-controller.getLeftX()); - double rotPercent = rotLimiter.calculate(-controller.getRightX()); + double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1); + double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1); + double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1); - // Apply deadband - double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xPercent, yPercent), 0.1); - Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); - double omega = MathUtil.applyDeadband(rotPercent, 0.1); - - // Calculate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); - - // Convert to field relative speeds & send command - drivetrain.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(), - omega * drivetrain.getMaxAngularSpeedRadPerSec(), - isFlipped - ? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180)) - : drivetrain.getRotation() - ) - ); + drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped); } @Override diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 373d06fa..e8e15ec1 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -125,6 +126,27 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); } + public void drivePercent(double xPercent, double yPercent, double rotPercent, boolean isFlipped) { + Rotation2d linearDirection = new Rotation2d(xPercent, yPercent); + double linearMagnitude = Math.hypot(xPercent, yPercent); + + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation(); + + // Convert to field relative + runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(), + rotPercent * getMaxAngularSpeedRadPerSec(), + isFlipped + ? getRotation().plus(Rotation2d.fromDegrees(180)) + : getRotation() + ) + ); + } + /** * Stops the drive. */