package com.edinarobotics.zed.commands;

import com.edinarobotics.zed.Components;
import com.edinarobotics.zed.subsystems.Lifter;
import edu.wpi.first.wpilibj.command.Command;

public class LiftShooterToAngleCommand extends Command {
    private Lifter lifter;
    private double angle;
    private boolean isOnTarget = false;
    
    private final double TOLERANCE = 5;
    
    public LiftShooterToAngleCommand(double angle) {
        super("LiftShooterToAngle");
        this.lifter = Components.getInstance().lifter;
        this.angle = angle;
    }
    
    protected void initialize() {
    }

    protected void execute() {
        if(angle > (lifter.getShooterAngle() - TOLERANCE)) {
            isOnTarget = false;
            lifter.setLifterDirection(Lifter.LifterDirection.LIFTER_UP);
        } else if(angle < (lifter.getShooterAngle() + TOLERANCE)) {
            isOnTarget = false;
            lifter.setLifterDirection(Lifter.LifterDirection.LIFTER_DOWN);
        } else {
            isOnTarget = true;
            lifter.setLifterDirection(Lifter.LifterDirection.LIFTER_STOP);
        }
    }

    protected boolean isFinished() {
        return isOnTarget;
    }

    protected void end() {
        lifter.setLifterDirection(Lifter.LifterDirection.LIFTER_STOP);
    }

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

@@ -277,52 +277,18 @@
 ble 
-angle;%0A    private boolean isOnTarget = fals
+targetAngl
 e;%0A 
@@ -329,12 +329,8 @@
 ANCE
- = 5
 ;%0A  
@@ -381,16 +381,34 @@
 le angle
+, double tolerance
 ) %7B%0A    
@@ -508,17 +508,23 @@
    this.
-a
+targetA
 ngle = a
@@ -525,24 +525,165 @@
 le = angle;%0A
+        TOLERANCE = tolerance;%0A        setTimeout(8);%0A    %7D%0A    %0A    public LiftShooterToAngleCommand(double angle)%7B%0A        this(angle, 2);%0A
     %7D%0A    %0A 
@@ -765,20 +765,29 @@
     
-if(a
+double lifterA
 ngle 
-%3E (
+= 
 lift
@@ -810,56 +810,90 @@
 le()
- - TOLERANCE)) %7B%0A            isOnTarget = false;
+;%0A        double liftError = lifterAngle - targetAngle;%0A        if(liftError %3E 0)%7B
 %0A   
@@ -961,10 +961,12 @@
 TER_
-UP
+DOWN
 );%0A 
@@ -969,24 +969,32 @@
 );%0A        %7D
+%0A       
  else if(ang
@@ -994,89 +994,23 @@
  if(
-angle %3C (lifter.getShooterAngle() + TOLERANCE)) %7B%0A            isOnTarget = false;
+liftError %3C 0)%7B
 %0A   
@@ -1066,36 +1066,34 @@
 irection.LIFTER_
-DOWN
+UP
 );%0A        %7D els
@@ -1092,46 +1092,22 @@
    %7D
- else %7B%0A            isOnTarget = true;
+%0A        else%7B
 %0A   
@@ -1244,25 +1244,156 @@
     
-return isOnTarget
+double lifterAngle = lifter.getShooterAngle();%0A        double liftError = lifterAngle - targetAngle;%0A        return Math.abs(liftError) %3C= TOLERANCE
 ;%0A  

Modify LiftShooterToAngleCommand to use the accelerometer.