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

// 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 * .02; // 20 ms
		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; }
}