| 6 | |
| 7 | |
| 8 | class PIDController(object): |
| 9 | def __init__(self, K_P=1.0, K_I=0.0, K_D=0.0, n=20): |
| 10 | self._K_P = K_P |
| 11 | self._K_I = K_I |
| 12 | self._K_D = K_D |
| 13 | |
| 14 | self._window = deque([0 for _ in range(n)], maxlen=n) |
| 15 | self._max = 0.0 |
| 16 | self._min = 0.0 |
| 17 | |
| 18 | def step(self, error): |
| 19 | self._window.append(error) |
| 20 | self._max = max(self._max, abs(error)) |
| 21 | self._min = -abs(self._max) |
| 22 | |
| 23 | if len(self._window) >= 2: |
| 24 | integral = np.mean(self._window) |
| 25 | derivative = (self._window[-1] - self._window[-2]) |
| 26 | else: |
| 27 | integral = 0.0 |
| 28 | derivative = 0.0 |
| 29 | |
| 30 | return self._K_P * error + self._K_I * integral + self._K_D * derivative |
| 31 | |
| 32 | class TCP(nn.Module): |
| 33 | |