def global_lookup_distance_index(path_distance_storage, path_x_storage,
path_y_storage, distance):
#
source_code = """
index = 0;
int i = 0;
for (i = 0; i < 100; ++i ) {
if ( path_distance_storage[i] < distance && path_distance_storage[i+1] > distance ) {
index = i;
break;
}
}
"""
array_type = dy.DataTypeArray(360, datatype=dy.DataTypeFloat64(1))
outputs = dy.generic_cpp_static(input_signals=[
path_distance_storage, path_x_storage, path_y_storage, distance
],
input_names=[
'path_distance_storage',
'path_x_storage', 'path_y_storage',
'distance'
],
input_types=[
array_type, array_type, array_type,
dy.DataTypeFloat64(1)
],
output_names=['index'],
output_types=[dy.DataTypeInt32(1)],
cpp_source_code=source_code)
index = outputs[0]
return index
name='velocity',
default_value=23.75,
value_range=[0, 25],
title="vehicle velocity")
k_p = dy.system_input(dy.DataTypeFloat64(1),
name='k_p',
default_value=2.0,
value_range=[0, 10.0],
title="controller gain")
disturbance_amplitude = dy.system_input(
dy.DataTypeFloat64(1),
name='disturbance_amplitude',
default_value=20.0,
value_range=[-45, 45],
title="disturbance amplitude") * dy.float64(math.pi / 180.0)
sample_disturbance = dy.system_input(dy.DataTypeInt32(1),
name='sample_disturbance',
default_value=50,
value_range=[0, 300],
title="disturbance position")
# parameters
wheelbase = 3.0
# sampling time
Ts = 0.01
# create storage for the reference path:
path = import_path_data(track_data)
# create placeholders for the plant output signals
# convert the transfer function T to the representation needed for dy.transfer_function_discrete
b, a = get_zm1_coeff(T)
# system input
u = dy.float64(1.0)
# implement the transfer function
y = dy.transfer_function_discrete(u, num_coeff=b, den_coeff=a ).set_name('y')
output_signals = [ y ]
if testname == 'switchNto1':
switch_state = dy.system_input( dy.DataTypeInt32(1) ).set_name('switch_state')
u1 = dy.float64(1.0)
u2 = dy.float64(2.0)
u3 = dy.float64(3.0)
u4 = dy.float64(4.0)
y = dy.switchNto1( state=switch_state, inputs=[u1,u2,u3,u4] )
output_signals = [ y ]
# NOTE: is this test still valid?
if testname == 'inline_subsystem':
def lookup_closest_point(N, path_distance_storage, path_x_storage,
path_y_storage, x, y):
"""
brute force implementation for finding a clostest point
"""
#
source_code = """
// int N = 360;
int N = *(&path_distance_storage + 1) - path_distance_storage;
int i = 0;
double min_distance_to_path = 1000000;
int min_index = 0;
for (i = 0; i < N; ++i ) {
double dx = path_x_storage[i] - x_;
double dy = path_y_storage[i] - y_;
double distance_to_path = sqrt( dx * dx + dy * dy );
if ( distance_to_path < min_distance_to_path ) {
min_distance_to_path = distance_to_path;
min_index = i;
}
}
double dx_p1, dy_p1, dx_p2, dy_p2, distance_to_path_p1, distance_to_path_p2;
dx_p1 = path_x_storage[min_index + 1] - x_;
dy_p1 = path_y_storage[min_index + 1] - y_;
distance_to_path_p1 = sqrt( dx_p1 * dx_p1 + dy_p1 * dy_p1 );
dx_p2 = path_x_storage[min_index - 1] - x_;
dy_p2 = path_y_storage[min_index - 1] - y_;
distance_to_path_p2 = sqrt( dx_p2 * dx_p2 + dy_p2 * dy_p2 );
int interval_start, interval_stop;
if (distance_to_path_p1 < distance_to_path_p2) {
// minimal distance in interval [min_index, min_index + 1]
interval_start = min_index;
interval_stop = min_index + 1;
} else {
// minimal distance in interval [min_index - 1, min_index]
interval_start = min_index - 1;
interval_stop = min_index;
}
// linear interpolation (unused)
double dx = path_x_storage[interval_stop] - path_x_storage[interval_start] ;
double dy = path_y_storage[interval_stop] - path_y_storage[interval_start] ;
index_start = interval_start;
index_closest = min_index;
distance = min_distance_to_path;
"""
array_type = dy.DataTypeArray(N, datatype=dy.DataTypeFloat64(1))
outputs = dy.generic_cpp_static(
input_signals=[
path_distance_storage, path_x_storage, path_y_storage, x, y
],
input_names=[
'path_distance_storage', 'path_x_storage', 'path_y_storage', 'x_',
'y_'
],
input_types=[
array_type, array_type, array_type,
dy.DataTypeFloat64(1),
dy.DataTypeFloat64(1)
],
output_names=['index_closest', 'index_start', 'distance'],
output_types=[
dy.DataTypeInt32(1),
dy.DataTypeInt32(1),
dy.DataTypeFloat64(1)
],
cpp_source_code=source_code)
index_start = outputs[0]
index_closest = outputs[1]
distance = outputs[2]
return index_closest, distance, index_start
#
# A vehicle controlled to follow a given path
#
system = dy.enter_system()
baseDatatype = dy.DataTypeFloat64(1)
# define simulation inputs
if not advanced_control:
velocity = dy.system_input( dy.DataTypeFloat64(1), name='velocity', default_value=7.2, value_range=[0, 25], title="vehicle velocity")
k_p = dy.system_input( dy.DataTypeFloat64(1), name='k_p', default_value=0.612, value_range=[0, 4.0], title="controller gain")
disturbance_amplitude = dy.system_input( dy.DataTypeFloat64(1), name='disturbance_amplitude', default_value=-11.0, value_range=[-45, 45], title="disturbance amplitude") * dy.float64(math.pi / 180.0)
sample_disturbance = dy.system_input( dy.DataTypeInt32(1), name='sample_disturbance', default_value=50, value_range=[0, 300], title="disturbance position")
z_inf_compensator = dy.system_input( dy.DataTypeFloat64(1), name='z_inf', default_value=0.9, value_range=[0, 1.0], title="z_inf_compensator")
if advanced_control:
velocity = dy.system_input( baseDatatype ).set_name('velocity').set_properties({ "range" : [0, 25], "unit" : "m/s", "default_value" : 23.75, "title" : "vehicle velocity" })
k_p = dy.system_input( baseDatatype ).set_name('k_p').set_properties({ "range" : [0, 4.0], "default_value" : 0.24, "title" : "controller gain" })
disturbance_amplitude = dy.system_input( baseDatatype ).set_name('disturbance_amplitude').set_properties({ "range" : [-45, 45], "unit" : "degrees", "default_value" : 0, "title" : "disturbance amplitude" }) * dy.float64(math.pi / 180.0)
sample_disturbance = dy.convert(dy.system_input( baseDatatype ).set_name('sample_disturbance').set_properties({ "range" : [0, 300], "unit" : "samples", "default_value" : 0, "title" : "disturbance position" }), target_type=dy.DataTypeInt32(1) )
distance_ahead = dy.system_input( baseDatatype ).set_name('distance_ahead').set_properties({ "range" : [0, 20.0], "default_value" : 5.0, "title" : "distance ahead" })
z_inf = dy.system_input( baseDatatype ).set_name('z_inf').set_properties({ "range" : [0, 1.0], "default_value" : 0.981, "title" : "z_inf" })
