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" })