Permalink
Cannot retrieve contributors at this time
thispath = get_absolute_file_path('MotionStim.sce'); | |
cd(thispath); | |
z = poly(0,'z'); | |
// | |
// This shows how to set-up two threads: One main thread with 300Hz | |
// and another one that is triggered every 12th sample by the main | |
// thread by a frequency division. | |
// | |
function [sim, outlist] = stimulation_freq_sche(sim, inlist) | |
// This superblock will run the evaluation of the experiment in a thread. | |
// The superblock describes a sub-simulation, whereby only one step is simulated | |
// which is enough to call scilab one signle time | |
defaultevents = 0; | |
ev = 0; | |
motion_downsampled = inlist(1); // only every 300/25'th value is taken from the high frequ original signal | |
[sim, zero] = ld_const(sim, ev, 0); | |
[sim] = ld_printf(sim, ev, in=zero, str="This is ~300/25Hz", insize=1); | |
// split up the whole motion vector | |
[sim, MO] = ld_demux(sim, ev, vecsize=24, invec=motion_downsampled); | |
yaw1 = MO(1); pitch1 = MO(2); roll1 = MO(3); | |
yaw2 = MO(4); pitch2 = MO(5); roll2 = MO(6); | |
Thu = MO(7); phiu = MO(8); Thf = MO(9); phif = MO(10); | |
Thud = MO(11); phiud = MO(12); Thfd = MO(13); phifd = MO(14); | |
Thudd = MO(15); phiudd = MO(16); Thfdd = MO(17); phifdd = MO(18); | |
[sim] = ld_print_angle(sim, ev, Thu, "Thu"); | |
[sim] = ld_print_angle(sim, ev, phiu, "phiu"); | |
[sim] = ld_print_angle(sim, ev, Thf, "Thf"); | |
[sim] = ld_print_angle(sim, ev, phif, "phif"); | |
// output of schematic | |
outlist = list(zero); | |
endfunction | |
// This is the main top level schematic | |
function [sim, outlist] = schematic_fn(sim, inlist) | |
// this is the default event | |
ev = 0; | |
// constants | |
[sim, zero] = ld_const(sim, ev, 0); [sim, one] = ld_const(sim, ev, 1); | |
[sim] = ld_printf(sim, ev, in=zero, str="This is ~300Hz", insize=1); | |
// A dummy for the vector of measurements | |
[sim, motion] = ld_constvec(sim, ev, vec=zeros(24,1) ); | |
// include a threaded simulation that is triggered at 25Hz | |
// input should be a signal vector of size 10 | |
// | |
// A freq divider | |
// | |
divisor = 300/25; // transform 300Hz into 25Hz | |
[sim, count ] = ld_modcounter(sim, ev, in=one, initial_count=5, mod=divisor); | |
[sim, trigger ] = ld_compare_01(sim, ev, in=count, thr=divisor-2+0.5); | |
trigger_stimulation = trigger; | |
// [sim] = ld_printf(sim, ev, in=count, str="count", insize=1); | |
// [sim] = ld_printf(sim, ev, in=trigger, str="trigger stimulation", insize=1); | |
// a nested simulation that runns asynchronously (in a thread) to the main simulation | |
[sim, outlist, computation_finished] = ld_simnest(sim, ev, ... | |
inlist=list(motion), ... | |
insizes=[ 24 ], outsizes=[ 1 ], ... | |
intypes=[ORTD.DATATYPE_FLOAT], outtypes=[ORTD.DATATYPE_FLOAT], ... | |
fn_list=list(stimulation_freq_sche), ... | |
dfeed=1, asynchron_simsteps=1, ... | |
switch_signal=zero, reset_trigger_signal=trigger_stimulation ); | |
output1 = outlist(1); | |
// computation_finished is one, when finished else zero | |
// output of schematic | |
[sim, out] = ld_const(sim, ev, 0); | |
outlist = list(out); // Simulation output #1 | |
endfunction | |
// | |
// Set-up | |
// | |
// defile events | |
defaultevents = [0]; // main event | |
// set-up schematic by calling the user defined function "schematic_fn" | |
insizes = [1,1]; outsizes=[1]; | |
[sim_container_irpar, sim]=libdyn_setup_schematic(schematic_fn, insizes, outsizes); | |
// | |
// Save the schematic to disk (possibly with other ones or other irpar elements) | |
// | |
parlist = new_irparam_set(); | |
// pack simulations into irpar container with id = 901 | |
parlist = new_irparam_container(parlist, sim_container_irpar, 901); | |
// irparam set is complete convert to vectors | |
par = combine_irparam(parlist); | |
// save vectors to a file | |
save_irparam(par, 'read_xs2_ArmMotion.ipar', 'read_xs2_ArmMotion.rpar'); | |
// clear | |
par.ipar = []; | |
par.rpar = []; | |
//// optionally execute | |
//messages=unix_g(ORTD.ortd_executable+ ' -s controller -i 901 -l 100'); | |
// | |
//// load results | |
//A = fscanfMat('result.dat'); | |
// | |
//scf(1);clf; | |
//plot(A(:,1), 'k'); | |