-
Notifications
You must be signed in to change notification settings - Fork 1
/
Controller.py
133 lines (117 loc) · 4.06 KB
/
Controller.py
1
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
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
# -*- coding: utf-8 -*-
"""
Created on Sat Sep 1 19:47:20 2018
@author: BrianPinto
"""
import abc
import numpy as np # pragma: no cover
# pylint: disable=too-few-public-methods
class Controller(object): # pragma: no cover
"""Base class for controllers. This defines the interface to controllers"""
__metaclass__ = abc.ABCMeta
@abc.abstractmethod
def reset_state(self):
"""
Reset the states of the controller to zero
"""
@abc.abstractmethod
def set_maxoutput(self):
"""
Set the maximal output of the Controller
"""
@abc.abstractmethod
def output(self, reference, system_output):
""" compute the controller output and return
Args:
reference (float): where the system should be
system_output (float): where the system actually is
Returns:
(float): controller_output for given input data
"""
return
class PidController(Controller):
"""
A simple PID controller
"""
def __init__(self, gain, tsampling, max_output):
"""
Args:
gain (list): gains of the diffrent parts of ctr
====== = ============
| gain = [Kp, Ti, Td]
====== = ============
Ts (float): sampling time of the controller / overall system
max_output (float): saturation of output
Example:
>>> Kp, Ti, Td = 10, 1.2, .4
>>> gain = [Kp, Ti, Td]
>>> tsampling, max_output = .001, 100
>>> controller = PidController(gain, tsampling, max_output)
"""
# Tuning Knobes
self.Kp = gain[0]
self.Ti = gain[1]
self.Td = gain[2]
self.max_output = max_output
self.integral = 0.
self.last_err = 0.
self.last_out = 0.
self.windup_guard = 0
self.gam = .1 # pole for stability. Typically = .1
self.tsampling = tsampling
self.initial_cable_length = None
def set_maxoutput(self, maxoutput):
self.max_output = maxoutput
def set_initial_cable_length(self, initial_cable_length):
self.initial_cable_length = initial_cable_length
def reset_state(self):
self.integral = 0.
self.last_err = 0.
self.windup_guard = 0.
self.last_out = 0.
def set_gain(self, gain):
self.Kp = gain[0]
self.Ti = gain[1]
self.Td = gain[2]
self.reset_state()
def output(self, reference, system_output):
"""
The controller output is calculated by:
========== = ======================================
| e = r-y
|
| ctr_out = Kp*e + Kp/Ti*integral(e) + Kp*Td*de/dt
========== = ======================================
Args:
reference (float): where the system should be
system_output (float): where the system actually is
Returns:
(float): controller_output
"""
# calc error
err = reference - system_output
# Derivative Anteil
# diff = (err - self.last_err)/self.tsampling
diff = (self.gam*self.Td - self.tsampling/2) / \
(self.gam*self.Td + self.tsampling/2) * \
self.last_out + \
self.Td/(self.gam+self.tsampling/2)*(err-self.last_err)
self.last_err = err
# Integral Anteil
integ = self.integral + self.tsampling / \
(2*self.Ti)*(err-self.windup_guard)
if np.abs(integ) > self.max_output:
integ = self.max_output*np.sign(integ)
self.integral = integ
# Sum
controller_output = self.Kp*(err + integ + diff)
if np.abs(controller_output) > self.max_output:
self.windup_guard = controller_output * \
(1-self.max_output/abs(controller_output))
self.last_out = self.max_output*np.sign(controller_output)
else:
self.windup_guard = 0
self.last_out = controller_output
return self.last_out
def sys_input(ctr_out):
return (.5 + ctr_out/2)*100