def __init__(self, maxAA, accelSpeed):
at_pid = PID2(0, 0, 0, -1, 1)
av_pid = PID2(0, 0, 0, -3, 3)
engine = Engine(self.maxThrust, accelSpeed, accelSpeed * 2)
self.atc = ATC(engine, self.lever,
self.maxThrust * self.lever / maxAA * 2, at_pid,
av_pid, 0.8)
def simAA(error,
maxAA,
engine,
base_thrust,
wheels_ratio,
error_rate=0,
error_time=0):
at_pid = PID2(1, 0.0, 1, 0, np.pi * 10)
av_pid = PID3(1, 0.0, 1, -1, 1, 3 * dt)
if wheels_ratio < 1:
MoI = engine.maxThrust * lever * 2 / maxAA / (
1 - wheels_ratio) * base_thrust
wheels_torque = maxAA * wheels_ratio * MoI
else:
base_thrust = 0
wheels_torque = engine.maxThrust * lever * 2
MoI = wheels_torque / maxAA
atc = ATC(engine, lever, MoI, at_pid, av_pid, base_thrust,
tune_steering, wheels_torque)
if error_rate > 0:
if error_time > 0:
return atc.simulate_random_attitude(error, error_rate,
error_time,
clampL(error * 2, 60))
return atc.simulate_linear_attitude(error, error_rate,
clampL(error * 2, 60))
return atc.simulate_static_attitude(error, clampL(error * 2, 60))
def sim_PID():
np.random.seed(42)
def rSV(val, delta):
return max(0, min(100,
val + (delta * np.random.random() - delta / 2.0)))
t1 = 10.0
PV = 10.0
SV = [rSV(50, 100)]
T = np.arange(0, t1, PID.dt)
sSV = SV[-1]
for _t in T[1:]:
r = np.random.random()
if r < 0.98:
SV.append(SV[-1])
continue
sSV = rSV(sSV, 50)
SV.append(sSV)
pid1 = PID(0.8, 0.2, 0.001, -10, 10)
pid2 = PID2(0.8, 0.2, 0.001, -10, 10)
pid1.sim(PV, SV, T)
pid2.sim(PV, SV, T)
plt.show()
def sim_Altitude():
# vs = loadCSV('VS-filtering-26.csv', ('AbsAlt', 'TerAlt', 'Alt', 'AltAhead', 'Err', 'VSP', 'aV', 'rV', 'dV'))
# # ter = vs.TerAlt[-10:10:-1]
# vs = vs[vs.Alt > 3]
# ter = vs.TerAlt[17000:22000]
sim1 = VSF_sim(AS=0.12, DS=0.5)
sim1.t1 = 60.0
thrust = [100] # np.arange(100, 300, 50)
def run_sim(c, n, A, sA, pid):
for t in thrust:
sim1.run_alt(A, sA, thrust=t, pid=pid)
i = 0;
r = 7
sim1.plot_alt(r, c, c * i + n)
i += 1
sim1.plot_ter(r, c, c * i + n)
i += 1
sim1.plot_ralt(r, c, c * i + n)
i += 1
sim1.plot_vs(r, c, c * i + n)
i += 1
sim1.plot_vsp(r, c, c * i + n)
i += 1
sim1.plot_dvsp(r, c, c * i + n)
i += 1
sim1.plot_k(r, c, c * i + n)
i += 1
sim1.describe()
# run_sim(2,1, 100.0, pid)
run_sim(2, 1, 50.0, 0.0, PID2(0.3, 0.0, 0.3, -9.9, 9.9))
run_sim(2, 2, 50.0, 105.0, PID2(0.3, 0.0, 0.3, -9.9, 9.9))
# run_sim(4,3, -alt/10.0, pid)
# run_sim(4,4, -alt, pid)
sim1.legend()
plt_show_maxed()
def simAA(error, maxAA, error_rate=0, error_time=0):
at_pid = PID2(1, 0, 0, 0, np.pi * 10)
av_pid = PID3(1, 0.1, 0, -1, 1, 3 * dt)
wheels_torque = maxAA
MoI = 1
atc = BRC(wheels_torque, MoI, at_pid, av_pid, tune_steering)
if error_rate > 0:
if error_time > 0:
return atc.simulate_random_attitude(error, error_rate,
error_time,
clampL(error * 2, 60))
return atc.simulate_linear_attitude(error, error_rate,
clampL(error * 2, 60))
return atc.simulate_static_attitude(error, clampL(error * 2, 60))
gamedir = u'/home/storage/Games/KSP_linux/PluginsArchives/Development/AT_KSP_Plugins/KSP-test/'
game = u'KSP_test_1.2.1'
gamedata = u'GameData'
def gamefile(filename):
return os.path.join(gamedir, game, filename)
def datafile(filename):
return os.path.join(gamedir, game, gamedata, filename)
wheesly = Engine.from_file(
datafile('Squad/Parts/Engine/jetEngines/jetEngineBasic.cfg'))
wiplash = Engine.from_file(
datafile('Squad/Parts/Engine/jetEngines/jetEngineTurbo.cfg'))
at_pid = PID2(2, 0.0, 0.1, -1, 1)
av_pid = PID2(1, 0.0, 1.1, -3, 3)
def square_lever(lever):
return np.sqrt(lever[0]**2 + lever[1]**2)
# Jet Hangar Test.AttitudeControl:
# MoI: (747.4614, 83.5321, 789.1782); | v | = 1090.174
# lever: (-1.83199, 3.203556, 3.024457); | v | = 4.771405
# maxThrust: 130,
# lever: (1.836533, 3.203599, 3.021605); | v | = 4.771372
# maxThrust: 130,
# lever: (-1.830195, -3.227458, 3.020665); | v | = 4.784403
# maxThrust: 130,
# lever: (1.836236, -3.225128, 3.017829); | v | = 4.783357
# maxThrust: 130