Added PelvicInflamatoryDisease

src/main/java/frc4388/robot/Robot.java

@@ -72,8 +72,7 @@ public class Robot extends TimedRobot {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    bot.setDefaultCommand(new AutoBalanceTF2(bot.frontLeft,
-      bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot));
+    bot.setDefaultCommand(new AutoBalanceTF2(bot));
   }
 
   /**

src/main/java/frc4388/robot/commands/AutoBalanceTF2.java

@@ -16,46 +16,35 @@ import edu.wpi.first.wpilibj2.command.PIDCommand;
 import frc4388.robot.Robot;
 import frc4388.utility.RobotGyro;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// NOTE:	Consider using this command inline, rather than writing a subclass.	For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class AutoBalanceTF2 extends PIDCommand {
-  // private TalonSRX frontLeft;
-  // private TalonSRX frontRight;
-  // private TalonSRX backLeft;
-  // private TalonSRX backRight;
+public class AutoBalanceTF2 extends PelvicInflamitoryDisease {
+	Robot.MicroBot bot;
 
-  /** Creates a new AutoBalanceTF2. */
-  public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) {
-    super(
-        // The controller that the command will use
-        new PIDController(.7, .02, .1),
-        // This should return the measurement
-        () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(),
-        // This should return the setpoint (can also be a constant)
-        () -> 0,
-        // This uses the output
-        output -> {
-          // Use the output here
-          double out2 = -MathUtil.clamp(output / 20, -1, 1);
-          if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
+	/** Creates a new AutoBalanceTF2. */
+	// ! finish integrating PelvicInflamatoryDisease
+	public AutoBalanceTF2(Robot.MicroBot bot) {
+		super(.7, .02, .1, 0);
+		addRequirements(bot);
+		this.bot = bot;
+	}
 
-          SmartDashboard.putNumber("Output", out2);
-          frontLeft.set(ControlMode.PercentOutput, out2);
-          frontRight.set(ControlMode.PercentOutput, out2);
-          backRight.set(ControlMode.PercentOutput, out2);
-          backLeft.set(ControlMode.PercentOutput, out2);
-        });
+	@Override
+	public double getError() {
+		return bot.gyro.getPitch();
+	}
 
-        gyro.reset();
-        addRequirements(bot);
-    // Use addRequirements() here to declare subsystem dependencies.
-    // Configure additional PID options by calling `getController` here.
-  }
+	@Override
+	public void runWithOutput(double output) {
+		double out2 = -MathUtil.clamp(output / 20, -1, 1);
+		if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0;
+		bot.setOutput(out2);
+	}
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
 }

src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.Gains;
+
+public abstract class PelvicInflamitoryDisease extends CommandBase {
+	protected Gains gains;
+	private double output = 0;
+
+	/** Creates a new PelvicInflamitoryDisease. */
+	public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) {
+		gains = new Gains(kp, ki, kd, kf, 0);
+	}
+
+	public PelvicInflamitoryDisease(Gains gains) {
+		this.gains = gains;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	/** figure it out bitch */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+		double error = getError();
+		cumError += error;
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		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;
+	}
+}