def generate_signal_PWM(period, modulator):
number_of_samples_to_stay_in_A = period * modulator
number_of_samples_to_stay_in_B = period * (dy.float64(1) - modulator)
number_of_samples_to_stay_in_A.set_name('number_of_samples_to_stay_in_A')
number_of_samples_to_stay_in_B.set_name('number_of_samples_to_stay_in_B')
with dy.sub_statemachine("statemachine1") as switch:
with switch.new_subsystem('state_on') as system:
on = dy.float64(1.0).set_name('on')
counter = dy.counter().set_name('counter')
timeout = (counter >=
number_of_samples_to_stay_in_A).set_name('timeout')
next_state = dy.conditional_overwrite(
signal=dy.int32(-1), condition=timeout,
new_value=1).set_name('next_state')
system.set_switched_outputs([on], next_state)
with switch.new_subsystem('state_off') as system:
off = dy.float64(0.0).set_name('off')
counter = dy.counter().set_name('counter')
timeout = (counter >=
number_of_samples_to_stay_in_B).set_name('timeout')
next_state = dy.conditional_overwrite(
signal=dy.int32(-1), condition=timeout,
new_value=0).set_name('next_state')
system.set_switched_outputs([off], next_state)
# define the outputs
pwm = switch.outputs[0].set_name("pwm")
state_control = switch.state.set_name('state_control')
return pwm, state_control
steering = delta_star + Delta_u
steering = dy.unwrap_angle(angle=steering, normalize_around_zero=True)
dy.append_output(Delta_u, 'Delta_u')
dy.append_output(delta_star, 'delta_star')
#
# The model of the vehicle including a disturbance
#
# model the disturbance
disturbance_transient = np.concatenate(
(cosra(50, 0, 1.0), co(10, 1.0), cosra(50, 1.0, 0)))
steering_disturbance, i = dy.play(
disturbance_transient,
start_trigger=dy.counter() == sample_disturbance,
auto_start=False)
# apply disturbance to the steering input
disturbed_steering = steering + steering_disturbance * disturbance_amplitude
# steering angle limit
disturbed_steering = dy.saturate(u=disturbed_steering,
lower_limit=-math.pi / 2.0,
upper_limit=math.pi / 2.0)
# the model of the vehicle
x_, y_, psi_, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(
disturbed_steering, velocity, Ts, wheelbase)
# close the feedback loops
# main simulation ouput
output_signals = [ output_x, output_v ]
if testname == 'loop_until':
baseDatatype = dy.DataTypeFloat64(1)
loop_iterations = dy.system_input( baseDatatype ).set_name('loop_iterations').set_properties({ "range" : [0, 100], "default_value" : 20 })
with dy.sub_loop( max_iterations=80 ) as system:
system.set_outputs([ dy.counter() ])
system.loop_until( dy.counter() >= loop_iterations )
counter = system.outputs[0]
# main simulation ouput
output_signals = [ counter ]
if testname == 'loop_yield_until':
baseDatatype = dy.DataTypeFloat64(1)
steering = dy.saturate(steering, delta_min, delta_max) steering = dy.rate_limit( steering, Ts, delta_dot_min, delta_dot_max, initial_state=steering ) delta_dot_hat = dy.diff( steering, initial_state=steering ) / dy.float64(Ts) dy.append_output( delta_dot_hat, 'delta_dot_hat' ) # # The model of the vehicle including a disturbance # # model the disturbance disturbance_transient = np.concatenate(( cosra(50, 0, 1.0), co(10, 1.0), cosra(50, 1.0, 0) )) steering_disturbance, _ = dy.play(disturbance_transient, start_trigger=dy.counter() == sample_disturbance, auto_start=False) steering_disturbance = steering_disturbance * disturbance_amplitude # # apply disturbance to the steering input # disturbed_steering = steering + steering_disturbance * disturbance_amplitude # # steering angle limit (model-intern) # disturbed_steering = dy.saturate(u=disturbed_steering, lower_limit=-math.pi/2.0, upper_limit=math.pi/2.0) # the model of the vehicle # x_, y_, psi_, x_dot, y_dot, psi_dot = discrete_time_bicycle_model(disturbed_steering, velocity, Ts, wheelbase) x_, y_, psi_, delta_, _delta_dot, a_lat_, _a_long, _x_dot, _y_dot, psi_dot_ = lateral_vehicle_model(steering, velocity, v_dot, Ts, wheelbase, delta_disturbance=steering_disturbance) # close the feedback loops
threshold_for_x_to_leave_B = dy.system_input( baseDatatype ).set_name('threshold_for_x_to_leave_B')
U2 = dy.system_input( baseDatatype ).set_name('osc_excitement')
# some modification of one input
U = U2 * dy.float64(1.234)
U.set_name("stachmachine_input_U")
with dy.sub_statemachine( "statemachine1" ) as switch:
with switch.new_subsystem('state_A') as system:
# implement a dummy system the produces zero values for x and v
x = dy.float64(0.0).set_name('x_def')
v = dy.float64(0.0).set_name('v_def')
counter = dy.counter().set_name('counter')
timeout = ( counter > number_of_samples_to_stay_in_A ).set_name('timeout')
next_state = dy.conditional_overwrite(signal=dy.int32(-1), condition=timeout, new_value=1 ).set_name('next_state')
system.set_switched_outputs([ x, v, counter ], next_state)
with switch.new_subsystem('state_B') as system:
# implement a simple spring-mass oscillator:
# x is the position, v is the velocity, acc is the acceleration
# create placeholder symbols for x and v (as they are used before being defined)
x = dy.signal()
v = dy.signal()