package org.usfirst.frc.team3501.robot.utils;
+import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.MathLib;
-public class PIDController {
+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 maxOutput;
private int minDoneCycleCount;
private int doneCycleCount;
+ private ITable table;
public PIDController() {
this(0.0, 0.0, 0.0, 0.0, 0.0);
this.iRange = eps;
}
- public void setSetPoint(double val) {
- this.setPoint = val;
+ public void setSetPoint(double VAL) {
+ this.setPoint = VAL;
}
public void setMaxOutput(double max) {
this.errorSum = 0.0;
}
- public double getDesiredVal() {
+ public double getDesiredVAL() {
return this.setPoint;
}
}
public double calcPIDError(double error) {
- double pVal = 0.0;
- double iVal = 0.0;
- double dVal = 0.0;
+ 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;
+ pVAL = this.pConst * error;
// + error outside of acceptable range which might distort the sum calc
if (error > this.iRange) {
}
// i contribution (final) calculation
- iVal = this.iConst * this.errorSum;
+ iVAL = this.iConst * this.errorSum;
// /////D Calc///////
double deriv = error - this.previousError;
- dVal = this.dConst * deriv;
+ dVAL = this.dConst * deriv;
// overal PID calc
- double output = pVal + iVal + dVal;
+ double output = pVAL + iVAL + dVAL;
// limit the output
output = MathLib.limitValue(output, this.maxOutput);
- // store current value as previous for next cycle
+ // store current VALue as previous for next cycle
this.previousError = error;
return output;
}
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)
+ || key.equals(Constants.DriveTrain.MOTOR_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),
+ table.getNumber(Constants.DriveTrain.MOTOR_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
+
+ }
}