DriveDistance.java

package frc.robot.commands.auto;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Gyro;

public class DriveDistance extends Command {

	Drivetrain drivetrain;
	Gyro gyro;
	Pose2d target;
	double xDistance;
	double yDistance;
	double theta;

	double xDisplacement;
	double yDisplacement;
	double angleDisplacement;

	double xyThreshold = 0.6;
	double xyMaxSpeed;
	double rotateMaxSpeed = 1.5;

    public DriveDistance(double xyMaxSpeed, Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) {
		this.drivetrain = drivetrain;
		this.gyro = gyro;
		this.xDistance = xDistance;
		this.yDistance = yDistance;
		this.theta = theta;
		this.xyMaxSpeed = xyMaxSpeed;
		addRequirements(drivetrain);

	}

	@Override
	public void initialize() {
	    System.out.println("drivedistance initialized");
		Pose2d start = drivetrain.getPose2d();
		this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta));
	}

	@Override
	public void execute() {
		// TODO: add conversion to inches if needed
		Pose2d currentPos = drivetrain.getPose2d();
		this.xDisplacement = target.getX() - currentPos.getX();
		double xSpeed = xDisplacement * 0.6;
		if (xSpeed > 0) {
			xSpeed = Math.min(xSpeed, xyMaxSpeed);
		} else {
			xSpeed = Math.max(xSpeed, -xyMaxSpeed);
		}
		// if (Math.abs(xDisplacement) <= xyThreshold) {
		// xSpeed = 0.0;
		// }

		SmartDashboard.putNumber("xDisplacement", xDisplacement);
		this.yDisplacement = target.getY() - currentPos.getY();
		double ySpeed = yDisplacement * 0.6;
		if (ySpeed > 0) {
			ySpeed = Math.min(ySpeed, xyMaxSpeed);
		} else {
			ySpeed = Math.max(ySpeed, -xyMaxSpeed);
		}
		// if (Math.abs(yDisplacement) <= xyThreshold) {
		// ySpeed = 0.0;
		// }
		SmartDashboard.putNumber("yDisplacement", yDisplacement);
		this.angleDisplacement = (gyro.getRotation2d().minus(target.getRotation())).getRadians();
		double rotateSpeed = angleDisplacement * 1.0;
		if (rotateSpeed > 0) {
			rotateSpeed = Math.min(rotateSpeed, rotateMaxSpeed);
		} else {
			rotateSpeed = Math.max(rotateSpeed, -rotateMaxSpeed);
		}

		SmartDashboard.putNumber("angleDisplacement", angleDisplacement);
		// System.out.println("x displacement: " + xDisplacement);
		// System.out.println("y displacement: " + yDisplacement);
		// System.out.println("angle displacement: ", angleDisplacement);
		drivetrain.drive(xSpeed, ySpeed, -rotateSpeed * 1.0);
	}

	@Override
	public void end(boolean interrupted) {
		drivetrain.drive(0.0, 0.0, 0.0);
	}

    @Override
    public boolean isFinished() {
	return (Math.abs(xDisplacement) <= xyThreshold && Math.abs(yDisplacement) <= xyThreshold && angleDisplacement <= 0.4);
    }

}