package com.edinarobotics.zeke.commands;

import com.edinarobotics.zeke.Components;
import com.edinarobotics.zeke.subsystems.Drivetrain;
import com.edinarobotics.zeke.subsystems.DrivetrainRotation;
import com.edinarobotics.zeke.subsystems.DrivetrainStrafe;
import edu.wpi.first.wpilibj.command.Command;

public class DriveXDistanceCommand extends Command {
    private Drivetrain drivetrain;
    private DrivetrainStrafe drivetrainStrafe;
    private DrivetrainRotation drivetrainRotation;
    private double distanceThreshold;
    
    private final double FORWARD_SPEED = 0.5;
    
    public DriveXDistanceCommand(double distanceThreshold) {
        super("DriveXDistanceCommand");
        this.drivetrain = Components.getInstance().drivetrain;
        this.drivetrainStrafe = Components.getInstance().drivetrainStrafe;
        this.drivetrainRotation = Components.getInstance().drivetrainRotation;
        this.distanceThreshold = distanceThreshold;
        requires(drivetrainStrafe);
        requires(drivetrainRotation);
    }
    
    protected void initialize() {
        drivetrainStrafe.setMecanumPolarStrafe(FORWARD_SPEED, 0.0);
        drivetrainRotation.setMecanumPolarRotate(0.0);
    }

    protected void execute() {
        drivetrainStrafe.setMecanumPolarStrafe(FORWARD_SPEED, 0.0);
        drivetrainRotation.setMecanumPolarRotate(0.0);
    }

    protected boolean isFinished() {
        return drivetrain.getUltrasonicSensor().getDistance() < distanceThreshold;
    }

    protected void end() {
        drivetrainStrafe.setMecanumPolarStrafe(0, 0);
        drivetrainRotation.setMecanumPolarRotate(0);
    }

    protected void interrupted() {
        end();
    }
}

@@ -35,16 +35,115 @@
 mands;%0A%0A
+import com.edinarobotics.utils.pid.PIDConfig;%0Aimport com.edinarobotics.utils.pid.PIDTuningManager;%0A
 import c
@@ -349,16 +349,60 @@
 Strafe;%0A
+import edu.wpi.first.wpilibj.PIDController;%0A
 import e
@@ -672,53 +672,178 @@
     
-%0A    private final double FORWARD_SPEED = 0.5
+private double speed;%0A    private boolean isForward;%0A    private final double P = 0.5, I = 0.0, D = 0.0, F = 0.0;%0A    PIDConfig pidConfig;%0A    PIDController pidController
 ;%0A  
@@ -902,16 +902,49 @@
 hreshold
+, double speed, boolean isForward
 ) %7B%0A    
@@ -1244,24 +1244,332 @@
 eThreshold;%0A
+        this.speed = speed;%0A        this.isForward = isForward;%0A        pidConfig = PIDTuningManager.getInstance().getPIDConfig(%22DriveXDistance%22);%0A        pidController = new PIDController(P, I, D, F, drivetrainRotation.getGyro(), drivetrainRotation, 0.025);%0A        pidController.setAbsoluteTolerance(5.0);%0A
         requ
@@ -1718,39 +1718,52 @@
 PolarStrafe(
-FORWARD_SPEED, 
+speed, isForward ? 90.0 : 27
 0.0);%0A      
@@ -1819,118 +1819,518 @@
     
-%7D%0A%0A    protected void execute() %7B%0A        drivetrainStrafe.setMecanumPolarStrafe(FORWARD_SPEED, 0.0);%0A        
+    drivetrainRotation.resetGyro();%0A        pidController.reset();%0A        pidController.enable();%0A    %7D%0A%0A    protected void execute() %7B%0A        drivetrainStrafe.setMecanumPolarStrafe(speed, isForward ? 90.0 : 270.0);%0A        pidConfig.setSetpoint(0.0);%0A        pidConfig.setValue(drivetrainRotation.getGyroAngle());%0A        pidController.setPID(pidConfig.getP(P), pidConfig.getI(I), pidConfig.getD(D), pidConfig.getF(F));%0A        pidController.setSetpoint(pidConfig.getSetpoint());%0A        System.out.println(
 driv
@@ -2348,33 +2348,64 @@
 ion.
-setMecanumPolarRotate(0.0
+getGyroAngle() + %22  %22 + drivetrainRotation.getRotation()
 );%0A 
@@ -2443,24 +2443,52 @@
 inished() %7B%0A
+        if(isForward) %7B%0A    
         retu
@@ -2566,37 +2566,184 @@
     
-%7D%0A%0A    protected void end() %7B
+    %7D else %7B%0A            return drivetrain.getUltrasonicSensor().getDistance() %3E distanceThreshold;%0A        %7D%0A    %7D%0A%0A    protected void end() %7B%0A        pidController.disable();
 %0A   

Add PID systems to DriveXDistanceCommand.