odometry

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

@@ -0,0 +1,58 @@
+// 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 PelvicInflammatoryDisease extends CommandBase {
+	protected Gains gains;
+	private double output = 0;
+
+	/** Creates a new PelvicInflamitoryDisease. */
+	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
+		gains = new Gains(kp, ki, kd, kf, 0);
+	}
+
+	public PelvicInflammatoryDisease(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; }
+}