This commit is contained in:
Ryan Manley
2023-01-14 15:23:48 -07:00
parent 913061546f
commit 745ab7bc35
@@ -8,52 +8,46 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.utility.Gains;
public abstract class PelvicInflamitoryDisease extends CommandBase {
protected Gains gains;
private double output = 0;
;;;;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);
}
;;;;/** 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;
}
;;;;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);
;;;;/** 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;
}
;;;;// 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;
;;;;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;
;;;;;;;;output = error * gains.kP;
;;;;;;;;output += cumError * gains.kI;
;;;;;;;;output += delta * gains.kD;
;;;;;;;;output += gains.kF;
runWithOutput(output);
}
;;;;;;;;runWithOutput(output);
;;;;}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
;;;;// 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;
}
;;;;// Returns true when the command should end.
;;;;@Override public boolean isFinished() { return false; }
}