def simulate(self, inputs, dt=None):
"""Simulate IMC.
Arguments:
----------
U : `numpy.ndarray`
2-dimensional array with two rows. First row U[0] contain a setting value,
second row U[1] contain a return value from plant/process.
time : `int`
Simulation number of iterations
Default ``time``=1
Returns:
--------
y : `numpy.ndarray`
Last controller's value
"""
u, yplant = inputs
u = Models.convert_2dim_array(u)
yplant = Models.convert_2dim_array(yplant)
yloop = Models.StateSpace.algebraic_loop_solver(
u - yplant, self.controller, self.model)
ymodel = self.model.simulate(inputs=yloop, dt=dt)
super().simulate(u + ymodel - yplant, dt=dt)
ycontrol = self.controller.simulate(inputs=super().now, dt=dt)
return ycontrol
def simulate(self, inputs, dt=1):
"""Simulate IMC.
Arguments:
----------
U : `numpy.ndarray`
2-dimensional array with two rows. First row U[0] contain a setting value,
second row U[1] contain a return value from plant/process.
time : `int`
Simulation number of iterations
Default ``time``=1
Returns:
--------
y : `numpy.ndarray`
Last controller's value
"""
u, yplant = inputs
u = Models.convert_2dim_array(u)
yplant = Models.convert_2dim_array(yplant)
ymodel = self.delay.now if self.delay else self.model.now
super().simulate(u + ymodel - yplant, dt=dt)
ycontrol = self.controller.simulate(inputs=super().now, dt=dt)
self.model.simulate(inputs=self.controller.now, dt=dt)
if self.delay:
self.delay.simulate(inputs=self.model.now, dt=dt)
return ycontrol
def __init__(self, Kp=0, Ti=0, Td=0, dt=1, N=float('inf'), **opt):
"""
Arguments:
----------
Kp : `float`
Proportional gain.
Default ``Kp``=1.
Ti : `float`
Integral time constant.
Default ``Ti``=0.
Td : `float`
Derivative time constant.
Default ``Td``=0.
dt : `float`
Sampling period.
Default ``dt``=1
N : `float`
Derivative filter coefficient
Default ``N``=float('inf')
"""
self.P_block = Models.Gain(Kp=Kp, dt=dt, **opt) if Kp else None
self.I_block = Models.Integral(Ki=1 / Ti, dt=dt, **opt) if Ti else None
self.D_block = None
if Td:
self.D_block = Models.Derivative(Kd=Td, dt=dt, **opt) if N == float('inf') \
else Models.StateSpace(ABCD=[[[-N / Td]], [[1]], [[- N ** 2 / Td]], [[N]]], dt=dt, **opt)
super().__init__(in_len=1, name='PID_Serial', **opt)
def trapezoid_func_F(th_e):
"""Calculate value of trapezoidal waveform for a given motor electrical angle.
Arguments
---------
th_e : `float`
Motor electrical angle in range [0, 2pi]
Returns
-------
F : `float`
Returns value of trapezoidal waveform for given angle
"""
tau = np.pi / 6
f = 2 / 3 * np.pi
T = np.pi
A = 1
if th_e <= tau:
F = th_e / tau
elif th_e <= f + tau:
F = A
elif th_e <= T:
F = A - A / tau * (th_e - (T - tau))
elif th_e <= T + tau:
F = -A / tau * (th_e - T)
elif th_e <= 2 * T - tau:
F = -1
elif th_e <= 2 * T:
F = -A + A / tau * (th_e - (2 * T - tau))
else:
raise Exception(
"Could not get trapezoid value for argument th={}".format(th_e))
return Models.convert_2dim_array(F)
def simulate(self, inputs, dt=None):
"""Simulation of a block.
Arguments:
----------
e : `float`, `numpy.ndarray`
Input to the controller as error e=u-y, where u is a setting value, y a return value from a process.
time : `int`
Number of simulation iterations.
Default ``time``=1
Return:
-------
y : `float`
Returns last value of simulation
"""
e = Models.convert_2dim_array(inputs).reshape(-1, 1)
yd = self.D_block.simulate(inputs=e, dt=dt) + e if self.D_block else e
yp = self.P_block.simulate(inputs=yd, dt=dt) if self.P_block else yd
yi = self.I_block.simulate(inputs=yp,
dt=dt) + yp if self.I_block else yp
super().simulate(yi)
return yi
def simulate(self, inputs, dt=None):
inputs = Models.convert_2dim_array(inputs).reshape(-1, 1)
ea, eb, ec = self.back_Emf.now[3:6]
uab, ubc, Tl = inputs
U = [[uab], [ubc], [ea - eb], [eb - ec], [self.torque.now], [Tl]]
super().simulate(inputs=U, dt=dt)
self.back_Emf.simulate(inputs=self.now[3:5], dt=dt) # [w, th_m]
self.torque.simulate(inputs=[self.back_Emf.now[0:3], self.now[0:3]],
dt=dt) # [Fa, Fb, Fc, ia, ib, ic]
def simulate(self, inputs, dt=None):
"""Simulate SP block.
Arguments:
----------
U : `numpy.ndarray`
2-dimensional array with two rows. First row [0] contain a setting value,
second [1] contain a return value from plant/process.
dt : `float`
Step time of simulation.
Default ``dt``=1
Returns:
--------
y : `numpy.ndarray`
Last controller's value
"""
u, yplant = inputs
u = Models.convert_2dim_array(u)
yplant = Models.convert_2dim_array(yplant)
yfilter = yplant - self.model_delay.now
if self.filter:
yfilter = self.filter.simulate(yfilter, dt)
ureg = u - yfilter
yloop = Models.StateSpace.algebraic_loop_solver(ureg,
self.controller,
self.model,
sign=-1)
ymodel = self.model.simulate(yloop, dt)
ycontrol = self.controller.simulate(ureg - ymodel, dt)
self.model_delay.simulate(ymodel, dt)
return ycontrol
def __init__(self, reg, model, delay, **opt):
"""
Arguments:
reg : `Models.Block`
Controller block.
model : `Models.Block`
Model of a process/plant.
opt : `dict`
``Models.Block`` options.
"""
self.controller = reg
self.model = model
self.delay = Models.Delay(d=delay, **
opt) if delay is not None else None
super().__init__(in_len=1, output_no_mem=True, **opt)
def __init__(self,
Kp=1,
Ti=0,
Td=0,
dt=1,
N=float('inf'),
alpha=None,
beta=None,
**opt):
"""
Arguments:
----------
Kp : `float`
Proportional gain.
Default ``Kp``=1.
Ti : `float`
Integral time constant.
Default ``Ti``=0.
Td : `float`
Derivative time constant.
Default ``Td``=0.
dt : `float`
Sampling period.
Default ``dt``=1
N : `float`
Derivative filter coefficient
Default ``N``=float('inf')
alpha : `float`
Alpha coefficient for two-degree of freedom controller.
Default ``alpha``=None
beta : `float`
Alpha coefficient for two-degree of freedom controller.
Default ``alpha``=None
"""
self.P_block = Models.Gain(Kp=Kp, dt=dt, **opt) if Kp else None
self.I_block = Models.Integral(Ki=Kp /
Ti, dt=dt, **opt) if Ti else None
self.D_block = None
self.P2_block = None
self.D2_block = None
if Td:
self.D_block = Models.Derivative(Kd=Td * Kp, dt=dt, **opt) if N == float('inf') \
else Models.StateSpace(ABCD=[[[-N / Td]], [[1]], [[- N ** 2 / Td]], [[N]]], dt=dt, K=Kp, **opt)
if alpha: self.P2_block = Models.Gain(Kp=Kp * alpha, **opt)
if beta:
self.D2_block = Models.Derivative(Kd=Kp * Td * beta, dt=dt, **opt) if N == float('inf') \
else Models.StateSpace(ABCD=[[[-N / Td]], [[1]], [[- N ** 2 / Td]], [[N]]], dt=dt, K=Kp * beta, **opt)
super().__init__(in_len=1, name='PID_Parallel', **opt)
def __init__(self, reg, model, delay, filter=None, **opt):
"""
Arguments:
---------
reg : `Models.Block`
Regulator represantation.
model : `Models.Block`
Model of a process.
delay : `int`
Dead time. Number of samples to be delayed.
filter : `Models.Block`, optional
Filtering block.
opt : `dict`, optional
`Model.Block` options.
"""
self.controller = reg
self.model = model
self.model_delay = Models.Delay(d=delay, **opt)
self.filter = filter
super().__init__(model='Smith-Predictor',
shape=(2, 1, 0),
model_func=None,
**opt)
def simulate(self, inputs, dt=None):
"""Simulation of a block.
Arguments:
----------
inputs : `float`, `numpy.ndarray`
Input to the controller as array ['e'] for 1-degree controller or ['e', 'u'] for 2-degree controller,
where 'u' is a setting value, 'e' is an error 'e = yplant - u' with plant output 'yplant'.
dt : `float`
Return:
-------
y : `float`
Returns last value from regulator.
"""
inputs = Models.convert_2dim_array(inputs).reshape(-1, 1)
if inputs.shape[0] == 1:
e = inputs
u = np.zeros((1, 1))
elif inputs.shape[0] == 2:
e, u = inputs
else:
raise Exception("PID: Expected max. 2 inputs. Got {}".format(
inputs.shape))
yp, yi, yd, yalfa, ybeta = np.zeros((5, 1, 1))
if self.P_block: yp = self.P_block.simulate(inputs=e, dt=dt)
if self.I_block: yi = self.I_block.simulate(inputs=e, dt=dt)
if self.D_block: yd = self.D_block.simulate(inputs=e, dt=dt)
if self.P2_block: yalfa = self.P2_block.simulate(inputs=u, dt=dt)
if self.D2_block: ybeta = self.D2_block.simulate(inputs=u, dt=dt)
y = yp + yi + yd + yalfa + ybeta
super().simulate(y, dt=dt)
return y
import matplotlib.pyplot as plt
from smacontrol import PID, Models
"""Simulation parameters"""
N = 100 # Simulation time's end
dt = 0.01 # Sampling time
max_len = int(N / dt) # Simulation's vectors' length
time = np.arange(0, N, dt) # Time vector
u = np.ones((1, max_len)) # Input vector u=1
input_on_time = 10 # Start time of input
u[0, :int(input_on_time / dt)] = 0 # Input u = 0 if t < input_on_time
"""State-Space model of inertia P(s) = 2/(50s+1)"""
A = [[-1 / 50]]
B = [[1]]
C = [[2 / 50]]
D = [[0]]
Plant_parallel = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len)
Plant_serial = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len)
Plant_open = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len)
"""Parallel and serial PID objects"""
Kp = 10
Ti = 2
Td = 0.2
Reg_PID_parallel = PID.PID(Kp=Kp, Ti=Ti, Td=Td, dt=dt, len=max_len)
Kps = Kp / 2 * np.sqrt(1 + 4 * Td / Ti)
Tis = Ti / 2 * np.sqrt(1 + 4 * Td / Ti)
Tds = Ti / 2 * np.sqrt(1 - 4 * Td / Ti)
Reg_PID_serial = PID.SerialPID(Kp=Kps, Ti=Tis, Td=Tds, dt=dt, len=max_len)
"""Simulation"""
for it in range(0, max_len):
t = time[it]
import matplotlib.pyplot as plt """Simulation parameters""" N = 100 # Simulation time's end dt = 0.01 # Sampling time max_len = int(N / dt) # Simulation's vectors' length time = np.arange(0, N, dt) # Time vector u = np.ones((1, max_len)) # Input vector u=1 input_on_time = 20 # Start time of input u[0, 0:int(input_on_time / dt)] = 0 # Input u = 0 if t < input_on_time """ Open loop - step response """ # Process P(s) = 12/(12s+1)e^-2 to state-space A, B, C, D = [[-1 / 12]], [[1]], [[1]], [[0]] d = int(2 / dt) Plant_openloop0 = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len) Delay_openloop0 = Models.Delay(d=d, dt=dt, len=max_len) """ ****** *** IMC 1 - model with delay ****** """ # Process P(s) = 12/(12s+1)e^-2s to state-space Plant1 = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len) Delay1 = Models.Delay(d=d, dt=dt, len=max_len) # Model Pm(s) = 12/(12s+1)e^-2s Plant_model1 = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len) # Regulator Q(s) = 12s+1/6s+12 Ar1, Br1, Cr1, Dr1 = [[-2]], [[1]], [[-23 / 6]], [[2]] Reg1 = Models.StateSpace(ABCD=[Ar1, Br1, Cr1, Dr1], dt=dt, len=max_len)
"""Simulation parameters""" N = 50 # Simulation time's end dt = 0.1 # Sampling time max_len = int(N / dt) # Simulation's vectors' length time = np.arange(0, N, dt) # Time vector u = np.ones((1, max_len)) # Input vector u=1 v = -1*np.ones((1, max_len)) # Disturbance vector v=-1, for t>=20 v_start = int(20 / dt) v[0, :v_start] = 0 """Open loop - step response""" # Process model P(s) = 1/(1s+1)e^-5 to state-space A, B, C, D = [[-1]], [[1]], [[1]], [[0]] d = int(5 / dt) Plant_openloop = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len) Delay_openloop = Models.Delay(d=d, dt=dt, len=max_len) """Smith Predictor""" # Process model P(s) = 1/(1s+1)e^-5 to state-space Plant = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len) Delay = Models.Delay(d=d, dt=dt, len=max_len) # Model Pmo(s) = 1/(1s+1) Model = Models.StateSpace(ABCD=[A, B, C, D], dt=dt, len=max_len) # Regulator C(s) = (1s + 1)/(0.5s) Ar, Br, Cr, Dr = [[0]], [[1]], [[2]], [[2]] Reg = Models.StateSpace(ABCD=[Ar, Br, Cr, Dr], dt=dt, len=max_len) # Output memory Mem = Models.Memory(in_len=1, dt=dt, len=max_len) # SmithPredictor structure SmithPred = SP.SmithPredictor(reg=Reg, model=Model, delay=d, len=max_len)