Fix some errors

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / utils / PIDController.java

package org.usfirst.frc.team3501.robot.utils;

import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.MathLib;

import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.tables.ITableListener;

public class PIDController implements LiveWindowSendable {

  private double pConst;
  private double iConst;
  private double dConst;
  private double setPoint;
  private double previousError;
  private double errorSum;
  private double errorIncrement;
  private double iRange;
  private double doneRange;
  private boolean firstCycle;
  private double maxOutput;
  private int minDoneCycleCount;
  private int doneCycleCount;
  private ITable table;

  public PIDController() {
    this(0.0, 0.0, 0.0, 0.0, 0.0);
  }

  public PIDController(double p, double i, double d, double eps,
      double iRange) {
    this.pConst = p;
    this.iConst = i;
    this.dConst = d;
    this.setPoint = 0.0;

    this.iRange = iRange;
    this.errorIncrement = 1.0;

    this.doneRange = eps;
    this.doneCycleCount = 0;
    this.minDoneCycleCount = 5;

    this.firstCycle = true;

    this.maxOutput = 1.0;

  }

  public PIDController(double p, double i, double d, double eps) {
    this(p, i, d, eps, eps * 0.8);
  }

  public PIDController(double p, double i, double d) {
    this(p, i, d, 1.0);
  }

  public void setConstants(double p, double i, double d) {
    this.pConst = p;
    this.iConst = i;
    this.dConst = d;
  }

  public void setConstants(double p, double i, double d, double eps,
      double iRange) {
    this.pConst = p;
    this.iConst = i;
    this.dConst = d;
    this.doneRange = eps; // range of error
    this.iRange = iRange;
  }

  public void setConstants(double p, double i, double d, double eps) {
    setConstants(p, i, d, eps, eps * 0.8);
  }

  public void setDoneRange(double range) {
    this.doneRange = range;
  }

  public void setIRange(double eps) {
    this.iRange = eps;
  }

  public void setSetPoint(double VAL) {
    this.setPoint = VAL;
  }

  public void setMaxOutput(double max) {
    if (max < 0.0) {
      this.maxOutput = 0.0;
    } else if (max > 1.0) {
      this.maxOutput = 1.0;
    } else {
      this.maxOutput = max;
    }
  }

  public void setMinDoneCycles(int num) {
    this.minDoneCycleCount = num;
  }

  public void resetErrorSum() {
    this.errorSum = 0.0;
  }

  public double getDesiredVAL() {
    return this.setPoint;
  }

  public double calcPID(double current) {
    return calcPIDError(this.setPoint - current);
  }

  public double calcPIDError(double error) {
    double pVAL = 0.0;
    double iVAL = 0.0;
    double dVAL = 0.0;

    if (this.firstCycle) {
      this.previousError = error;
      this.firstCycle = false;
    }

    pVAL = this.pConst * error;

    // + error outside of acceptable range which might distort the sum calc
    if (error > this.iRange) {
      // check if error sum was in the wrong direction
      if (this.errorSum < 0.0) {
        this.errorSum = 0.0;
      }
      // only allow up to the max contribution per cycle
      this.errorSum += Math.min(error, this.errorIncrement);
    } // - error outside of acceptable range
    else if (error < -1.0 * this.iRange) {
      // error sum was in the wrong direction
      if (this.errorSum > 0.0) {
        this.errorSum = 0.0;
      }
      // add either the full error or the max allowable amount to sum
      this.errorSum += Math.max(error, -1.0 * this.errorIncrement);
    }

    // i contribution (final) calculation
    iVAL = this.iConst * this.errorSum;

    // /////D Calc///////
    double deriv = error - this.previousError;
    dVAL = this.dConst * deriv;

    // overal PID calc
    double output = pVAL + iVAL + dVAL;

    // limit the output
    output = MathLib.limitValue(output, this.maxOutput);

    // store current VALue as previous for next cycle
    this.previousError = error;
    return output;
  }

  public boolean isDone() {
    double currError = Math.abs(this.previousError);

    // close enough to target
    if (currError <= this.doneRange) {
      this.doneCycleCount++;
    }
    // not close enough to target
    else {
      this.doneCycleCount = 0;
    }

    return this.doneCycleCount >= this.minDoneCycleCount;
  }

  private final ITableListener m_listener = (table, key, value, isNew) -> {
    if (key.equals(Constants.DriveTrain.P_VAL)
        || key.equals(Constants.DriveTrain.I_VAL)
        || key.equals(Constants.DriveTrain.D_VAL)) {
      if (getP() != table.getNumber(Constants.DriveTrain.P_VAL, 0.0)
          || getP() != table.getNumber(Constants.DriveTrain.I_VAL, 0.0)
          || getD() != table.getNumber(Constants.DriveTrain.D_VAL, 0.0)) {
        setConstants(table.getNumber(Constants.DriveTrain.P_VAL, 0.0),
            table.getNumber(Constants.DriveTrain.I_VAL, 0.0),
            table.getNumber(Constants.DriveTrain.D_VAL, 0.0));
      }
    } else if (key.equals(Constants.DriveTrain.SETPOINT)) {
      if (getSetpoint() != (Double) value) {
        setSetPoint((Double) value);
      }
    }
  };

  @Override
  public void initTable(ITable table) {

    if (this.table != null) {
      table.removeTableListener(m_listener);
    }
    this.table = table;
    if (table != null) {
      table.putNumber(Constants.DriveTrain.P_VAL, getP());
      table.putNumber(Constants.DriveTrain.I_VAL, getI());
      table.putNumber(Constants.DriveTrain.D_VAL, getD());
      table.putNumber(Constants.DriveTrain.SETPOINT, getSetpoint());
      table.addTableListener(m_listener, false);
    }

  }

  private double getP() {
    return this.pConst;
  }

  private double getI() {
    return this.iConst;
  }

  private double getD() {
    return this.dConst;
  }

  private double getSetpoint() {
    return this.setPoint;
  }

  @Override
  public ITable getTable() {
    // TODO Auto-generated method stub
    return null;
  }

  @Override
  public String getSmartDashboardType() {
    return "Drive Controller";
  }

  @Override
  public void updateTable() {

  }

  @Override
  public void startLiveWindowMode() {
    // TODO Auto-generated method stub

  }

  @Override
  public void stopLiveWindowMode() {
    // TODO Auto-generated method stub

  }
}