From 6027b5ffb22a43b20baa393f69c3bf88db5a4c1e Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 14 Mar 2023 10:47:17 -0600
Subject: [PATCH] added more arm command stuff

---
 .../frc4388/robot/commands/ArmCommand.java    | 55 +++++++------------
 .../frc4388/robot/commands/AutoBalance.java   |  2 +-
 .../commands/PelvicInflammatoryDisease.java   | 23 ++++----
 3 files changed, 33 insertions(+), 47 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ArmCommand.java b/src/main/java/frc4388/robot/commands/ArmCommand.java
index 6afe6b4..87ab2b1 100644
--- a/src/main/java/frc4388/robot/commands/ArmCommand.java
+++ b/src/main/java/frc4388/robot/commands/ArmCommand.java
@@ -4,58 +4,43 @@
 
 package frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.opencv.osgi.OpenCVInterface;
+
 import frc4388.robot.subsystems.Arm;
 import frc4388.robot.subsystems.Claw;
 
 public class ArmCommand extends PelvicInflammatoryDisease {
-  private Arm arm;
-  private Claw claw;
-  private boolean toggle;
+  private final Arm     arm;
+  private final Claw    claw;
+  private final boolean toggle;
+  private final double  target;
   
   /** Creates a new ArmCommand. */
-  public ArmCommand(Arm arm, Claw claw, boolean open) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    super(0.6, 0, 0, 0);
-    this.arm = arm;
-    this.claw = claw;
+  public ArmCommand(Arm arm, Claw claw, double target, boolean open) {
+    super(0.6, 0, 0, 0, 0);
+    this.arm    = arm;
+    this.claw   = claw;
     this.toggle = open;
+    this.target = target;
     addRequirements(arm, claw);
   }
 
-  public ArmCommand(Arm arm, Claw claw) {
-    this(arm, claw, claw.isClawOpen());
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    super.initialize();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
+  public ArmCommand(Arm arm, Claw claw, double target) {
+    this(arm, claw, target, claw.isClawOpen());
   }
 
   @Override
   public double getError() {
-    // TODO Auto-generated method stub
-    return 0;
+    return (arm.getArmRotation() - target) / 360;
   }
 
   @Override
   public void runWithOutput(double output) {
-    // TODO Auto-generated method stub
-    
+    arm.setRotVel(output);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    claw.setClaw(toggle);
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java
index 2da529c..1c3523f 100644
--- a/src/main/java/frc4388/robot/commands/AutoBalance.java
+++ b/src/main/java/frc4388/robot/commands/AutoBalance.java
@@ -16,7 +16,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 
 	/** Creates a new AutoBalanceTF2. */
 	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
-		super(0.6, 0, 0, 0);
+		super(0.6, 0, 0, 0, 0);
 
 		this.gyro = gyro;
 		this.drive = drive;
diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
index fb6a871..7135326 100644
--- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
@@ -8,16 +8,19 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.utility.Gains;
 
 public abstract class PelvicInflammatoryDisease extends CommandBase {
-	protected Gains gains;
-	private double output = 0;
+	protected Gains  gains;
+	private   double output    = 0;
+	private   double tolerance = 0;
 
 	/** Creates a new PelvicInflammatoryDisease. */
-	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
-		gains = new Gains(kp, ki, kd, kf, 0);
+	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
+		gains          = new Gains(kp, ki, kd, kf, 0);
+		this.tolerance = tolerance;
 	}
 
-	public PelvicInflammatoryDisease(Gains gains) {
-		this.gains = gains;
+	public PelvicInflammatoryDisease(Gains gains, double tolerance) {
+		this.gains     = gains;
+		this.tolerance = tolerance;
 	}
 
 	/** produces the error from the setpoint */
@@ -48,11 +51,9 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 		runWithOutput(output);
 	}
 
-	// Called once the command ends or is interrupted.
-	@Override
-	public void end(boolean interrupted) {}
-
 	// Returns true when the command should end.
 	@Override
-	public boolean isFinished() { return false; }
+	public boolean isFinished() {
+		return Math.abs(getError()) < tolerance;
+	}
 }