From: EvanYap <evanyap.14@gmail.com>
Date: Sat, 4 Feb 2017 22:22:58 +0000 (-0800)
Subject: do lots of stuff wit hlivewindowsendable
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=424c070c238f1fd8e989330aca3e5704a2bac8df;p=3501%2F2017steamworks

do lots of stuff wit hlivewindowsendable
---

diff --git a/src/org/usfirst/frc/team3501/robot/utils/PIDController.java b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
index c6ab0fb..2e40d7a 100644
--- a/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
+++ b/src/org/usfirst/frc/team3501/robot/utils/PIDController.java
@@ -1,8 +1,13 @@
 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;
@@ -17,6 +22,7 @@ public class PIDController {
   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);
@@ -77,8 +83,8 @@ public class PIDController {
     this.iRange = eps;
   }
 
-  public void setSetPoint(double val) {
-    this.setPoint = val;
+  public void setSetPoint(double VAL) {
+    this.setPoint = VAL;
   }
 
   public void setMaxOutput(double max) {
@@ -99,7 +105,7 @@ public class PIDController {
     this.errorSum = 0.0;
   }
 
-  public double getDesiredVal() {
+  public double getDesiredVAL() {
     return this.setPoint;
   }
 
@@ -108,16 +114,16 @@ public class PIDController {
   }
 
   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) {
@@ -138,19 +144,19 @@ public class PIDController {
     }
 
     // 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;
   }
@@ -169,4 +175,85 @@ public class PIDController {
 
     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
+
+  }
 }