Ejemplo n.º 1
0
 public void initTable(ITable table) {
   if (this.table != null) {
     this.table.removeTableListener(listener);
   }
   this.table = table;
   if (table != null) {
     table.putNumber("p", gains.getP());
     table.putNumber("i", gains.getI());
     table.putNumber("d", gains.getD());
     table.putNumber("f", gains.getF());
     table.putNumber("goal", goal);
     table.putNumber("source", source.get());
     table.putBoolean("enabled", isEnabled());
     table.addTableListener(listener, false);
   }
 }
Ejemplo n.º 2
0
 public void update() {
   lastSource = source.get();
   double out = 0;
   double error = goal - lastSource;
   double p = gains.getP() * error;
   if (Math.abs(error) < minIError) errorSum += error;
   double i = gains.getI() * errorSum;
   double dError = error - lastError;
   double d = gains.getD() * dError;
   lastError = error;
   double ff = gains.getF() * goal;
   if (enabled) {
     out = ff + p + i + d;
     output.set(out);
     lastOut = out;
   }
   lastDeltaError = dError;
   SmartDashboard.putData(this);
   System.out.println(enabled + " " + error + " " + out);
 }