def filter(self, input_):
self.filter_array = np.roll(self.filter_array, 1)
self.filter_array[0] = input_
return np.average(self.filter_array)
# (speed, time)
speed_u = [(0, 0), (3, 20), (5.5, 140), (5, 250)] #, (0, 450)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future: add noise
t_end = 600
pid_speed = PID(P=1.0, I=0.1, D=1., Ibounds_speed=(0, 600.3)) # -0.04646
#
u_ref = []
u_real = []
rpm_set = []
periods = 1
rpm_array = MA_filter(periods)
# rpm_1_set = MA_filter(periods)
# rpm_2_set = MA_filter(periods)
while t < t_end:
# calculate speed input
if end_input == 0:
if speed_u[speed_u_position][1] <= t:
# def __init__(self, periods):
# self.filter_array = np.zeros(periods)
# def filter(self, input_):
# self.filter_array = np.roll(self.filter_array, 1)
# self.filter_array[0] = input_
# return np.average(self.filter_array)
# (speed, time)
input_sequence = [(10, 0), (40, 20), (350, 140), (10, 250), (50, 450)]
end_input = 0
hdg_position = 0
current_hdg_setting = 0
t = 0
dt = 0.4 #future: add noise
t_end = 200
pid_hdg = PID(P=1.0, I=0.0, D=15.0, Ibounds_speed=(-90,90)) # -0.04646
max_deg_second = 15
#
u_ref = []
u_real = []
rpm_set = []
periods = 1
while t<t_end:
# calculate speed input
if end_input==0:
if input_sequence[hdg_position][1] <= t:
current_hdg_setting = input_sequence[hdg_position][0]
pid_hdg.setPoint_hdg(current_hdg_setting)
hdg_position += 1
import time
# time.sleep(3)
coef_ = np.genfromtxt('borkum_general_opt_.csv', delimiter=',')
ship = ship()
input_ = [
(0, 0, 0, 0), (50.0, 50.0, 45.0, 1)
] #, (50.0, 50.0,225, 50)]#, (0.0, 50.,270.0, 150)]#,(-50.0, -50.,270.0, 250)]#,(-50.0, 0.,180.0, 300),(-50.0, 0.,180.0, 450)]#, (0.0, 0,290, 120)]#, (50., 100., 100)]#,(100., 100., 200),(100., 0., 300)]#], (0., 100., 180)]#, (0., 0., 190)]#, (20,70), (10, 150)]
end_input = 0
input_position = 0
t = 0
dt = 1. #future noise
t_end = 520
pid_position_x = PID(P=0.6, I=0.0, D=25.)
pid_position_y = PID(P=0.6, I=0.0, D=25.)
pid_attitude = PID(P=.1, I=0., D=16.5)
# pid_position_x = PID(P=2.5, I=0.0, D=50.)
# pid_position_y = PID(P=2.5, I=0.0, D=50.)
# pid_attitude = PID(P=.85, I=0.0, D=25.0)
# pid_attitude = PID(P=.265, I=0.0, D=.9)
ship_model = ship_model(0, 0, 0, ship, coef_)
x, x_old = 0, 0
y, y_old = 0, 0
u, v, r, hdg = 0.0, 0, 0, 45
rpm_1, rpm_2 = 0, 0
input_ref = []
traj_x, traj_y, hdg_list = [], [], []
def PID_tuner(x):
#initialise ship
u, v, r, hdg = 0, 0, 0, 0
ship_ = ship()
ship_model_ = ship_model(0, 0, 0, ship_, coef_)
max_rpm_second = 0.25
rpm = 0
# (speed, time)
speed_u = [(0, 0), (5, 20), (9, 140), (2, 250)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future noise
t_end = 400
pid_speed = PID(P=x[0], I=x[1], D=x[2])
#
u_ref = []
u_real = []
while t < t_end:
# calculate speed input
if end_input == 0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_speed.setPoint_speed(current_speed_setting)
speed_u_position += 1
if speed_u_position == len(speed_u):
end_input = 1
control_input = pid_speed.update(u)
sign_control_input = np.sign(control_input)
# calculate speed control iput
if abs(abs(control_input) - abs(rpm)) / dt > max_rpm_second:
rpm = dt * sign_control_input * max_rpm_second + rpm
if abs(rpm) > abs(pid_speed.Integrator_max):
rpm = np.sign(rpm) * abs(pid_speed.Integrator_max)
# print(rpm)
# model
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_model_.manoeuvre_model_rpa_3(
u,
v,
r,
hdg,
rpm,
0, #rudder
dt,
)
u_ref.append(current_speed_setting)
u_real.append(u)
t = t + dt
u_ref_array = np.asarray(u_ref)
u_real_array = np.asarray(u_real)
u_score = abs(u_ref_array - u_real_array)
u_score = u_score.sum()
print(u_score)
return u_score
def PID_tuner(x):
#initialise ship
u, v, r, hdg = 0, 0, 0, 0
ship_ = ship()
ship_model_ = ship_model(0, 0, 0, ship_, coef_)
max_rpm_second = 0.25
rpm = 0
# (distance, time)
speed_u = [(0, 0), (10, 40)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future noise
t_end = 400
pid_position = PID(P=x[0], I=x[1], D=x[2], Ibounds_speed=(-90, 90))
#
u_ref = []
u_real = []
x = 0
while t < t_end:
# calculate speed input
if end_input == 0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_position.setPoint_speed(current_speed_setting)
speed_u_position += 1
if speed_u_position == len(speed_u):
end_input = 1
control_input = pid_position.update(x)
sign_control_input = np.sign(control_input)
# calculate speed control iput
if abs(abs(control_input) - abs(rpm)) / dt > max_rpm_second:
rpm = dt * sign_control_input * max_rpm_second + rpm
if abs(rsa_1) > abs(pid_position.Integrator_max):
rpm = np.sign(rpm) * abs(pid_position.Integrator_max)
# print(rpm)
# model
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_model.manoeuvre_model_rt_evolution(
u,
v,
r,
hdg,
0,
rpm_1,
rpm_2,
180,
rsa_1,
rsa_2,
dt,
# u_dot_arr, v_dot_arr, r_rot_arr
)
x = x + delta_y_0
u_ref.append(current_speed_setting)
u_real.append(u)
t = t + dt
u_ref_array = np.asarray(u_ref)
u_real_array = np.asarray(u_real)
u_score = abs(u_ref_array - u_real_array)
u_score = u_score.sum()
print(u_score)
return u_score
import time
time.sleep(3)
coef_ = np.genfromtxt('foo_evo_general.csv', delimiter=',')
ship = ship()
input_ = [
(0, 0, 0, 0), (80.0, 0.0, 0.0, 1)
] #, (50.0, 50.,90.0, 150),(50.0, 50.,270.0, 250),(-50.0, 0.,180.0, 300),(-50.0, 0.,180.0, 450)]#, (0.0, 0,290, 120)]#, (50., 100., 100)]#,(100., 100., 200),(100., 0., 300)]#], (0., 100., 180)]#, (0., 0., 190)]#, (20,70), (10, 150)]
end_input = 0
input_position = 0
t = 0
dt = 1.4 #future noise
t_end = 180
pid_position_x = PID(P=.3, I=0.0, D=10., Ibounds_speed=(-90, 90))
pid_position_y = PID(P=.3, I=0.00, D=10., Ibounds_speed=(-90, 90))
pid_attitude = PID(P=0.065, I=0.0, D=1.9, Ibounds_speed=(-90, 90))
ship_model = ship_model(0, 0, 0, ship, coef_)
x, x_old = 0, 0
y, y_old = 0, 0
u, v, r, hdg = 0.0, 0, 0, 0
rpm_1, rpm_2 = 0, 0
input_ref = []
traj_x, traj_y, hdg_list = [], [], []
last_coordinate = [0, 0]
sign_attitude_control = 1
current_input_setting_attitude = 0
#initialise ship
u, v, r, hdg = 0, 0, 0, 0
ship_model = ship_model(0, 0, 0, ship, coef_)
max_rpm_second = 0.3
rpm = 0
# (speed, time)
speed_u = [(0, 0), (3, 20), (7, 140), (4, 250), (9, 450)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future: add noise
t_end = 600
pid_speed = PID(P=25.6, I=-0.04, D=2.653) # -0.04646
#
u_ref = []
u_real = []
rpm_set = []
while t < t_end:
# calculate speed input
if end_input == 0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_speed.setPoint_speed(current_speed_setting)
speed_u_position += 1
if speed_u_position == len(speed_u):
end_input = 1
#initialise ship
u, v, r, hdg = 0,0,0,0
ship_model = ship_model(0,0,0, ship, coef_)
max_rpm_second = 0.5
rpm = 0
# (speed, time)
speed_u = [(0, 0), (1.0,10), (1.5, 140), (3.5, 250), (0, 450)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future: add noise
t_end = 600
pid_speed = PID(P=5., I=.6, D=2.0) # -0.04646
#
u_ref = []
u_real = []
rpm_set = []
while t<t_end:
# calculate speed input
if end_input==0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_speed.setPoint_speed(current_speed_setting)
speed_u_position += 1
def PID_tuner(x):
#initialise ship
u, v, r, hdg = 0,0,0,0
max_rpm_second = 1.2
rpm = 0
# (speed, time)
speed_u = [(0, 0), (3, 20), (6, 140), (5, 250), (1, 450)]
end_input = 0
speed_u_position = 0
current_speed_setting = 0
t = 0
dt = 0.4 #future: add noise
t_end = 600
pid_speed = PID(P=x[0], I=x[1], D=x[2], Ibounds_speed=(0,6.3)) # -0.04646
#
u_ref = []
u_real = []
rpm_set = []
periods = 1
rpm_array = MA_filter(periods)
# rpm_1_set = MA_filter(periods)
# rpm_2_set = MA_filter(periods)
while t<t_end:
# calculate speed input
if end_input==0:
if speed_u[speed_u_position][1] <= t:
current_speed_setting = speed_u[speed_u_position][0]
pid_speed.setPoint_speed(current_speed_setting)
speed_u_position += 1
if speed_u_position==len(speed_u):
end_input = 1
control_input = pid_speed.update(u)
sign_control_input = np.sign(control_input)
# calculate speed control iput
if abs(abs(control_input)-abs(rpm))/dt>max_rpm_second:
rpm = dt*sign_control_input*max_rpm_second + rpm
if abs(rpm)>abs(pid_speed.Integrator_max):
rpm = np.sign(rpm)*abs(pid_speed.Integrator_max)
# elif rpm<
# print(rpm)
# model
rpm_0, rpm_1, rpm_2 = rpm_array.filter(rpm), rpm_array.filter(rpm), rpm_array.filter(rpm)
rsa_0,rsa_1,rsa_2 = 180,180,180
if rpm<0.0:
rpm_0 ,rpm_1, rpm_2 = 1.0,1.0,1.0
rsa_0,rsa_1,rsa_2 = 0,0,0
# if rpm>0.0:
# rpm_1, rpm_2, rpm_3 = rpm+8.8, rpm+8.8, rpm+8.8
# else:
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_model.manoeuvre_model_rt_evolution(u, v, r, hdg,
rpm_0, rpm_1, rpm_2,
rsa_0,rsa_1,rsa_2,#rudder
dt,
)
u_ref.append(current_speed_setting)
u_real.append(u)
rpm_set.append(rpm)
t = t + dt
u_ref_array = np.asarray(u_ref)
u_real_array = np.asarray(u_real)
u_score = abs(u_ref_array-u_real_array)
u_score = u_score.sum()
print(u_score)
return u_score
def PID_tuner(x):
u, v, r, hdg = 0, 0, 0, 0
input_sequence = [(0, 0), (40, 20), (350, 140), (10, 250), (120, 450)]
end_input = 0
hdg_position = 0
current_hdg_setting = 0
t = 0
dt = 0.5 #future: add noise
t_end = 600
pid_hdg = PID(P=x[0], I=0., D=x[1], Ibounds_speed=(-90, 90)) # -0.04646
max_deg_second = 5
#
u_ref = []
u_real = []
rpm_set = []
periods = 1
while t < t_end:
# calculate speed input
if end_input == 0:
if input_sequence[hdg_position][1] <= t:
current_hdg_setting = input_sequence[hdg_position][0]
pid_hdg.setPoint_hdg(current_hdg_setting)
hdg_position += 1
if hdg_position == len(input_sequence):
end_input = 1
# print(hdg)
control_input = pid_hdg.update(hdg)
sign_hdg_diff = np.sign(current_hdg_setting - hdg)
# print(control_input)
if abs(current_hdg_setting - hdg) > 180.:
# print('a')
control_input = control_input * -1
# print(control_input)
sign_control_input = np.sign(control_input)
if abs(control_input) > 35.:
control_input = sign_control_input * 35
rpm = 10.
rpm_0, rpm_1, rpm_2 = rpm, rpm, rpm
# print(control_input)
rsa_1, rsa_2 = 180, 180
rsa_0 = 180 + control_input
if abs(control_input - rsa_0) / dt > max_deg_second:
rsa_0 = dt * sign_control_input * max_deg_second + rsa_0
u, v, r, hdg, delta_x_0, delta_y_0, delta_r_0, u_dot, v_dot, r_dot = ship_model.manoeuvre_model_borkum(
u,
v,
r,
hdg,
rpm_0,
rpm_1,
rpm_2,
rsa_0,
rsa_1,
rsa_2, #rudder
dt,
)
u_ref.append(current_hdg_setting)
u_real.append(hdg)
rpm_set.append(rpm)
t = t + dt
u_ref_array = np.asarray(u_ref)
u_real_array = np.asarray(u_real)
u_score = abs(u_ref_array - u_real_array)
u_score = u_score.sum()
print(u_score)
plt.plot(u_ref)
plt.plot(u_real)
# plt.plot(rpm_set)
plt.savefig('tuner_borkum.png')
plt.show()
return u_score