diff --git a/src/main/java/frc/robot/commands/teleop/DistanceToTargetCommand.java b/src/main/java/frc/robot/commands/teleop/DistanceToTargetCommand.java new file mode 100644 index 00000000..11fdbb74 --- /dev/null +++ b/src/main/java/frc/robot/commands/teleop/DistanceToTargetCommand.java @@ -0,0 +1,103 @@ +package frc.robot.commands.teleop; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +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.util.Units; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.Constants; +import frc.robot.commands.OdometryHeadingCommand; +import frc.robot.commands.interfaces.AutoDrivableCommand; +import frc.robot.coordinates.PolarCoordinate; +import frc.robot.subsystems.AutoDrive; +import frc.robot.subsystems.Heading; + +/** + * Drive a certain distance from a point - regardless of angle - and maintain that + * distance away from the point. Will attempt to face target while driving towards + * target. + */ +public class DistanceToTargetCommand extends ParallelCommandGroup { + + public DistanceToTargetCommand(double distanceMeters, Supplier poseSupplier, AutoDrive autoDrive, Heading heading) { + this(Constants.kHub, distanceMeters, poseSupplier, autoDrive, heading); + } + + public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters, Supplier poseSupplier, AutoDrive autoDrive, Heading heading) { + super( + new DistanceToPointCommand(targetMeters, distanceMeters, poseSupplier, autoDrive), + new OdometryHeadingCommand(targetMeters, poseSupplier, heading) + ); + } + +} + +class DistanceToPointCommand extends CommandBase implements AutoDrivableCommand { + private Translation2d targetMeters; + private double distanceMeters; + private Supplier poseSupplier; + private AutoDrive autoDrive; + + private PIDController distanController = new PIDController(1.0, 0, 0); + + private double output = 0.0; + + public DistanceToPointCommand(Translation2d targetMeters, double distanceMeters, Supplier poseSupplier, AutoDrive autoDrive) { + this.targetMeters = targetMeters; + this.distanceMeters = distanceMeters; + this.poseSupplier = poseSupplier; + this.autoDrive = autoDrive; + + addRequirements(autoDrive); + } + + @Override + public void initialize() { + autoDrive.setDelegate(this); + } + + @Override + public void execute() { + // Find our robot polar coordinate relative to the target + PolarCoordinate robotCoordinate = PolarCoordinate.fromFieldCoordinate( + poseSupplier.get().getTranslation(), + targetMeters + ); + output = distanController.calculate( + robotCoordinate.getRadiusMeters(), + distanceMeters + ); + + // Clamp to some max speed (should be between [0.0, 1.0]) + final double maxSpeed = 0.3; + output = MathUtil.clamp( + output, + -maxSpeed, + maxSpeed + ); + + // TODO: This should probably be a field-oriented movement based on the + // robot's heading. This will allow us to move indepdently of the heading. + } + + @Override + public void end(boolean interrupted) { + autoDrive.clearDelegate(); + } + + public AutoDrive.State calculate(double forward, double strafe, boolean isFieldOriented) { + // Use our distanController output calculation as our forward value. + // Driver can strafe during this command. Forward should be forward to the robot. + // Note that this command assumes we're facing the target (use with Heading) + return new AutoDrive.State( + -output, + strafe, + false + ); + } +}