mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-09 00:37:59 -06:00
fixes
This commit is contained in:
@@ -8,46 +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;
|
||||
;;;;}
|
||||
|
||||
;;;;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; }
|
||||
// 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; }
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user