PID.emam 606 Bytes
Newer Older
Bernhard Rumpe's avatar
BR-sy    
Bernhard Rumpe committed
1
/* (c) https://github.com/MontiCore/monticore */
Jean-Marc Ronck's avatar
Jean-Marc Ronck committed
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
package de.rwth.armin.modeling.autopilot.common;

component PID {
  port
    in Q paramP,
    in Q paramI,
    in Q paramD,
    in Q paramDecayCoefficient,
    in Q error,
    out Q control;

  implementation Math {
    static B isPrevErrorSpecified = 0;
    static Q prevError = 0.0;
    static Q acc = 0.0;
    acc = paramDecayCoefficient * acc + error;
    Q drv = 0;
    if isPrevErrorSpecified
      drv = error - prevError;
    end
    control = paramP * error + paramI * acc + paramD * drv;
    prevError = error;
    isPrevErrorSpecified = 1;
  }
}