mirror of
https://github.com/NickNair/Adaptive-PID-controller.git
synced 2026-02-07 02:52:25 +08:00
75 lines
1.4 KiB
Python
75 lines
1.4 KiB
Python
import numpy as np
|
|
from numpy.core.numeric import NaN
|
|
from scipy.linalg import expm
|
|
import math
|
|
|
|
class SingleArea:
|
|
|
|
def __init__(self,Tg,Tt,M,D,R,T,yt_1,yt_2,yt_3):
|
|
|
|
self.yt_1 = yt_1
|
|
self.yt_2 = yt_2
|
|
self.yt_3 = yt_3
|
|
|
|
self.Xprev = np.zeros( (3,1) )
|
|
self.Y = np.zeros( (1,1) )
|
|
|
|
self.Tg = Tg
|
|
self.Tt = Tt
|
|
self.M = M
|
|
self.D = D
|
|
self.R = R
|
|
|
|
self.T = T
|
|
|
|
self.CalcDiscreteCoef()
|
|
|
|
def CalcDiscreteCoef(self):
|
|
|
|
# Calculating Continous coef
|
|
|
|
self.A = np.array( [ [-self.D/self.M , 1/self.M , 0] , [ 0 , -1/self.Tt , 1/self.Tt ] , [-1/( self.Tg*self.R ) , 0 , -1/self.Tg ] ] )
|
|
|
|
self.B = np.array( [ [0 , -1/self.M ] , [ 0 , 0 ] , [ 1/self.Tg , 0 ] ])
|
|
|
|
self.C = np.array( [[1 , 0 , 0]] )
|
|
|
|
# Calculating Discrete Coefs
|
|
|
|
self.Ad = expm(self.A*self.T)
|
|
|
|
# Add check later
|
|
|
|
self.Bd = np.dot( np.dot(np.linalg.inv(self.A),(self.Ad - np.eye(3) )), self.B )
|
|
|
|
def Output(self,Ut):
|
|
|
|
self.yt_1 , self.yt_2 , self.yt_3 = self.Y[0,0] , self.yt_1, self.yt_2
|
|
|
|
self.X = np.dot( self.Ad, self.Xprev ) + np.dot( self.Bd, Ut )
|
|
|
|
self.Y = np.dot( self.C, self.Xprev)
|
|
|
|
# print("Chooth :" , self.Y)
|
|
|
|
self.Xprev = self.X
|
|
|
|
if (math.isnan(self.Y[0,0])):
|
|
|
|
return True
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|