Select Git revision
Forked from
ACS / Public / Teaching materials / Systemtheorie 2 / lecture-tutorials
Source project has a limited visibility.
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
Block.py 20.22 KiB
import numpy
sin = numpy.sin
from random import triangular
#########################################################################
#### Abstract class for continuous time block in a block diagram ########
#########################################################################
class Block(object):
def __init__(self):
self.ninput = 0
self.noutput = 0
self.nstate = 0
self.nparam = 0
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.x = numpy.zeros((self.nstate))
self.xo = numpy.zeros((self.nstate))
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
def Init(self,initvalue):
self.x = initvalue
def ChangeParameters(self,value):
value = 0
def Reset(self):
self.x = zeros(self.nstate)
def GetInput(self,sc):
for i in range(0,self.ninput):
self.u[i]=sc.flows[self.inpos[i]]
def WriteOutput(self,sc):
for i in range(0,self.noutput):
sc.flows[self.outpos[i]]=self.y[i]
def Step(self,t,dt):
for i in range(0,self.nstate):
self.x[i] = self.x[i]
def cdxdt(self,x,t):
dxdt = numpy.zeros((self.nstate))
return dxdt
def updatestate(self,t,dt):
dx1 = self.cdxdt(self.x,t)
xp = numpy.zeros((self.nstate))
for i in range(0,self.nstate):
xp[i] = self.x[i]+dx1[i]*dt
dx2 = self.cdxdt(xp,t+dt)
for i in range(0,self.nstate):
self.x[i] = self.x[i]+(dx1[i]+dx2[i])/2*dt
#########################################################################
################### Step function as a signal source ####################
#########################################################################
class StepSource(Block):
def __init__(self,out,startv,endv,ts):
super(StepSource, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.startv = startv
self.endv = endv
self.ts = ts
self.y = numpy.zeros((self.noutput))
self.y[0] = startv
def Step(self,t,dt):
if t>=self.ts:
self.y[0] = self.endv
#########################################################################
####################### Sinusoidal signal source ########################
#########################################################################
class SinusoidalSignalSource(Block):
def __init__(self,out,startv,Am,om,phi):
super(SinusoidalSignalSource, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.Am = Am
self.om = om
self.phi = phi
self.y = numpy.zeros((self.noutput))
self.y[0] = startv
def Step(self,t,dt):
self.y[0] = self.Am*sin(self.om*t+self.phi)
#########################################################################
########################### Unknown wave source #########################
#########################################################################
class UnknownWaveSource(Block):
def __init__(self,out,startv,Am,om,phi):
super(UnknownWaveSource, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.Am = Am
self.om = om
self.phi = phi
self.y = numpy.zeros((self.noutput))
self.y[0] = startv
def Step(self,t,dt):
#Alternativ: self.y[0] = self.Am*sin(otp)+0.1*self.Am*sin(otp*0.1)
opt = (self.om*t)+self.phi
self.y[0] = self.Am*(sin(opt)-0.3*(sin(opt))**2+0.2*(sin(opt-1))**2)
#########################################################################
################################## Noise ################################
#########################################################################
class Noise(Block):
def __init__(self,inp,out,Am):
super(Noise, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.ninput = 1
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.inpos[0] = inp
self.Am = Am
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
#self.y[0] = startv
def Step(self,t,dt):
noise = ([self.Am*triangular(0.0,0.5,1.0)-(self.Am/2.0)])
self.y = self.u + numpy.array(noise)
#########################################################################
######################### Square signal source ##########################
#########################################################################
class SquareSignal(Block):
def __init__(self,out,hi,lo,f,duty):
super(SquareSignal, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.hi = hi
self.lo = lo
self.f = f
self.Ts = 1/f
self.duty = duty
self.y = numpy.zeros((self.noutput))
def Step(self,t,dt):
d = t/self.Ts-int(t/self.Ts);
if d > self.duty:
self.y[0] = self.lo;
else:
self.y[0] = self.hi;
#########################################################################
##################### Constant as a signal source #######################
#########################################################################
class Constant(Block):
def __init__(self,out,value):
super(Constant, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.y = numpy.zeros((self.noutput))
self.y[0] = value
#########################################################################
########################## Division operation ##########################
#########################################################################
class Division(Block):
def __init__(self,in1,in2,out):
super(Division, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.ninput = 2
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.inpos[0] = in1
self.inpos[1] = in2
self.y[0] = 0
def Step(self,t,dt):
self.y[0] = self.u[0]/self.u[1]
#########################################################################
########################## Product operation ###########################
#########################################################################
class Product(Block):
def __init__(self,in1,in2,out):
super(Product, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.ninput = 2
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.inpos[0] = in1
self.inpos[1] = in2
self.y = numpy.zeros((self.noutput))
self.y[0] = 0
def Step(self,t,dt):
self.y[0] = self.u[0]*self.u[1]
#########################################################################
############################# Sum operation ############################
#########################################################################
class Sum(Block):
def __init__(self,in1,in2,out,sign1,sign2):
super(Sum, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.ninput = 2
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.inpos[0] = in1
self.inpos[1] = in2
self.sign1 = sign1
self.sign2 = sign2
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
self.y[0] = 0
def Step(self,t,dt):
self.y[0] = self.u[0]*self.sign1+self.u[1]*self.sign2
#########################################################################
############################## Saturation ##############################
#########################################################################
class Saturation(Block):
def __init__(self,inp,out,minout,maxout):
super(Saturation, self).__init__()
self.noutput = 1
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.outpos[0] = out
self.ninput = 1
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.inpos[0] = inp
self.minout = minout
self.maxout = maxout
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
def Step(self,t,dt):
self.y[0] = self.u[0]
if self.y[0] > self.maxout:
self.y[0] = self.maxout
if self.y[0] < self.minout:
self.y[0] = self.minout
def ChangeParameters(self,value):
self.minout = value[0]
self.maxout = value[1]
#########################################################################
############################# Gain block ###############################
#########################################################################
class Gain(Block):
def __init__(self,inp,out,k):
super(Gain, self).__init__()
self.ninput = len(inp)
self.noutput = len(out)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
for i in range(0,self.ninput):
self.inpos[i] = inp[i]
for i in range(0,self.noutput):
self.outpos[i] = out[i]
self.k = numpy.zeros((self.noutput,self.ninput))
self.k = k
def Step(self,t,dt):
self.y = self.k.dot(self.u.transpose())
def ChangeParameters(self,value):
self.k = value
#########################################################################
########################## PI Controller ###############################
#########################################################################
class PI(Block):
def __init__(self,inp,out,Kp,Ki,ini):
super(PI, self).__init__()
self.ninput = 1
self.noutput = 1
self.nstate = 1
self.A = 0
self.B = 1
self.C = Ki
self.D = Kp
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.x = numpy.zeros((self.nstate))
self.xo = numpy.zeros((self.nstate))
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
self.inpos[0] = inp
self.outpos[0] = out
self.x[0] = ini
self.xo[0] = ini
def Step(self,t,dt):
self.y[0] = self.C*self.x[0]+self.D*self.u[0]
self.updatestate(t,dt)
def Reset(self):
self.x[0] = xo[0]
def cdxdt(self,x,t):
dxdt = numpy.zeros((self.nstate))
dxdt[0] = self.B*u[0]
return dxdt
def ChangeParameters(self,value):
self.C = np.array([value[1]])
self.D = np.array([value[0]])
#########################################################################
########################## PID Controller ##############################
#########################################################################
class PID(Block):
def __init__(self,inp,out,Kp,Ki,Kd,ini):
super(PID, self).__init__()
self.ninput = 1
self.noutput = 1
self.nstate = 1
self.B = 1
self.C = Ki
self.D = Kp
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.x = numpy.zeros((self.nstate))
self.xo = numpy.zeros((self.nstate))
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
self.inpos[0] = inp
self.outpos[0] = out
self.x[0] = ini
self.xo[0] = ini
self.olderr = 0
self.Kd = Kd
def Step(self,t,dt):
self.y[0] = self.C*self.x[0]+self.D*self.u[0]
self.y[0] = self.y[0]+self.Kd*(self.u[0]-self.olderr)/dt
self.olderr = self.u[0]
self.updatestate(t,dt)
def Reset(self):
self.x[0] = xo[0]
self.olderr = 0;
def cdxdt(self,x,t):
dxdt = numpy.zeros((self.nstate))
dxdt[0] = self.B*self.u[0]
return dxdt
def ChangeParameters(self,value):
self.C = value[1]
self.D = value[0]
self.Kd = value[2]
#########################################################################
############################# Integrator ################################
#########################################################################
class Integrator(Block):
def __init__(self,inp,out,ini):
super(Integrator, self).__init__()
self.ninput = 1
self.noutput = 1
self.nstate = 1
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.x = numpy.zeros((self.nstate))
self.xo = numpy.zeros((self.nstate))
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
self.inpos[0] = inp
self.outpos[0] = out
self.x[0] = ini
self.xo[0] = ini
def Step(self,t,dt):
self.y[0] = self.x[0]
self.updatestate(t,dt)
def Reset(self):
self.x[0] = xo[0]
def cdxdt(self,x,t):
dxdt = numpy.zeros((self.nstate))
dxdt[0] = self.u[0]
return dxdt
#########################################################################
################################ Not ####################################
#########################################################################
class Not(Block):
def __init__(self,inp,out):
super(Not, self).__init__()
self.ninput = 1
self.noutput = 1
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
self.inpos[0] = inp
self.outpos[0] = out
def Step(self,t,dt):
if self.u[0] == 0:
self.y[0] = 1
else:
self.y[0] = 0
#########################################################################
########################## State Space #################################
#########################################################################
class StateSpace(Block):
def __init__(self,inp,out,A,B,C,D,xo):
super(StateSpace, self).__init__()
self.ninput = len(inp)
self.noutput = len(out)
self.nstate = len(A)
self.A = A
self.B = B
self.C = C
self.D = D
self.inpos = inp
self.outpos = out
self.x = numpy.zeros((self.nstate))
self.xo = numpy.zeros((self.nstate))
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
for i in range(0,self.nstate):
self.x[i] = xo[i]
self.xo[i] = xo[i]
def Step(self,t,dt):
for i in range(0,self.noutput):
self.y[i] = 0
for j in range(0,self.nstate):
if self.C.ndim == 1:
self.y[i] = self.y[i] + self.C[j]*self.x[j]
elif self.C.ndim == 2:
#prevent [ValueError: setting an array element with a sequence.] for 2 dimensional self.C:
self.y[i] = self.y[i] + self.C[i][j]*self.x[j]
else:
print("WARNING: Can not handle self.C with dimension "+str(self.C.ndim))
for j in range(0,self.ninput):
if self.D.ndim == 1:
self.y[i] = self.y[i] + self.D[j]*self.u[j]
elif self.D.ndim == 2:
#prevent [ValueError: setting an array element with a sequence.] for 2 dimensional self.C:
self.y[i] = self.y[i] + self.D[i][j]*self.u[j]
else:
print("WARNING: Can not handle self.D with dimension "+str(self.C.ndim))
self.updatestate(t,dt)
def Reset(self):
for i in range(0,self.nstate):
self.x[i] = xo[i]
def cdxdt(self,x,t):
dxdt = numpy.matmul(self.B,self.u.T) + numpy.matmul(self.A,self.x)
return dxdt
#########################################################################
####################### Transfer Function ##############################
#########################################################################
class TransferFunction(Block):
def __init__(self,inp,out,num,den):
super(TransferFunction, self).__init__()
self.ninput = 1
self.noutput = 1
self.inpos = numpy.zeros((self.ninput),dtype=numpy.int)
self.outpos = numpy.zeros((self.noutput),dtype=numpy.int)
self.inpos[0] = inp
self.outpos[0] = out
self.nstate = len(den)-1
denord = len(den)
numord = len(num)
self.A = numpy.zeros((self.nstate,self.nstate))
self.B = numpy.zeros((self.nstate))
self.C = numpy.zeros((self.nstate))
self.D = 0
scale = den[0];
for i in range(0,denord):
den[i]= den[i]/scale
for i in range(0,numord):
num[i]= num[i]/scale
if numord==denord:
self.D = num[0]
for i in range(0,numord-1):
self.C[i]=num[numord-i-1]-num[0]*den[numord-i-1]
else:
for i in range(0,numord):
self.C[i]=num[numord-i-1]
for i in range(0,self.nstate):
self.A[self.nstate-1,i] = -den[denord-i-1]
for i in range(0,self.nstate-1):
self.A[i,i+1]=1
self.B[self.nstate-1] = 1
self.x = numpy.zeros((self.nstate))
self.xo = numpy.zeros((self.nstate))
self.u = numpy.zeros((self.ninput))
self.y = numpy.zeros((self.noutput))
for i in range(0,self.nstate):
self.x[i] = 0
self.xo[i] = 0
def Step(self,t,dt):
self.y[0] = self.D*self.u[0]
for i in range (0,self.nstate):
self.y[0] = self.y[0]+self.C[i]*self.x[i]
self.updatestate(t,dt)
def Reset(self):
for i in range(0,self.nstate):
self.x[i] = xo[i]
def cdxdt(self,x,t):
dxdt = numpy.zeros((self.nstate))
for i in range (0,self.nstate):
dxdt[i] = self.B[i]*self.u[0]
for j in range (0,self.nstate):
dxdt[i] = dxdt[i] + self.A[i,j]*x[j]
return dxdt