27
Приложение В Программа модели SRS и ППП
Файл srs.py. Часть 1: параметры, пределы тяги обеих осей, шаг RK4 и разгон. TR —
тестовый режим, RR — рабочий, IDLE — неподвижный холостой ход.
from math import exp, log
G, F, C, R, TAU = 9.81, .015, 40., .34, .25
BETA, ALPHA, MU = .55, .50, .65
TC, DEC, DV, DELAY = 6., 120., 2.4, .003
AP = 30.
P0, TR, TD = 80000., .008, .070
def params(mode):
m, demand = (2000., 900.) if mode == 'TR' else (2400., 1800.)
if mode == 'IDLE': demand = 0.
cap = min(MU*BETA*m*G/ALPHA,
MU*(1-BETA)*m*G/(1-ALPHA))
force = min(demand/R, cap)
return m, force
def rk4(fun, t, y, h):
a = fun(t, y)
b = fun(t+h/2, [v+h*w/2 for v,w in zip(y,a)])
c = fun(t+h/2, [v+h*w/2 for v,w in zip(y,b)])
d = fun(t+h, [v+h*w for v,w in zip(y,c)])
return [v+h*(a0+2*b0+2*c0+d0)/6
for v,a0,b0,c0,d0 in zip(y,a,b,c,d)]
def drive(mode, h=.01, end=TC):
m, force = params(mode)
f0 = F*m*G
if force <= f0: return [[0.,0.,0.],[end,0.,0.]]
t0 = -TAU*log(1-f0/force)
def rhs(t,y):
return [y[1], (force*(1-exp(-t/TAU))-f0-C*y[1])/m]
t, y = t0, [0.,0.]
rows = [[0.,0.,0.], [t,*y]]
while t < end-1e-12:
dt = min(h,end-t)
y = rk4(rhs,t,y,dt); t += dt
rows.append([t,*y])
return rows