コード例 #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);
   }
 }
コード例 #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);
 }
コード例 #3
0
 public void valueChanged(ITable table, String key, Object value, boolean isNew) {
   if (key.equals("p") || key.equals("i") || key.equals("d") || key.equals("f")) {
     if (gains.getP() != table.getNumber("p", 0.0)
         || gains.getI() != table.getNumber("i", 0.0)
         || gains.getD() != table.getNumber("d", 0.0)) {
       System.out.println("Got new PID gains!");
       gains.set(
           table.getNumber("p", 0.0), table.getNumber("i", 0.0), table.getNumber("d", 0.0));
     }
   } else if (key.equals("setpoint")) {
     if (goal != ((Double) value).doubleValue()) {
       setGoal(((Double) value).doubleValue());
     }
   } else if (key.equals("enabled")) {
     if (isEnabled() != ((Boolean) value).booleanValue()) {
       if (((Boolean) value).booleanValue()) {
         enable();
       } else {
         disable();
       }
     }
   }
 }