[METEORIT] pre PID analisas

sim2_A.m

sim_duration = 20;            # simulation duration, seconds
sim_freq = 150;               # simulation step frequency
target_position_A = 1200;     # initial positoin of target, DILEC
target_position_B = 5700;     # initial positoin of target, DILEC
head_position_current = 4000; # initial head position, DILEC
head_speed_current = 0;

PARAM_K_SPEED = 170 / 1000;
INTEGRATOR_SPEED = (1.0 / 0.016); # 16 [mV]  = 1 [dc / s]
output_voltage_MIN = -10;
output_voltage_MAX = 10;
head_speed_tau = 0.9;         # head rotation damping ratio

# --------------------------------

# number of steps to simultate
sim_steps = sim_duration * sim_freq;

# timeline
timeline = linspace(0, sim_duration, sim_steps);

% Preallocate arrays
target_position = zeros(0, sim_steps);
head_position = zeros(0, sim_steps);
output_voltage = zeros(0, sim_steps);
head_speed = zeros(0, sim_steps);

# --------------------------------------

for step = 1:sim_steps

    target_position_current = target_position_A + step * (target_position_B - target_position_A) / sim_steps;
    target_position(step) = target_position_current;

    # find difference between head and target position
    d_head_position = target_position_current - head_position_current;
    # calc output voltage
    output_voltage_current = d_head_position * PARAM_K_SPEED;
    # clip it working range
    output_voltage_current(output_voltage_current < output_voltage_MIN) = output_voltage_MIN;
    output_voltage_current(output_voltage_current > output_voltage_MAX) = output_voltage_MAX;
    # save
    output_voltage(step) = output_voltage_current;

    # update head position depending on it current speed
    head_position_current = head_position_current +  head_speed_current * (1 / sim_freq);
    head_position(step) = head_position_current;
    # update head rotating speed with tau factor
    head_speed = INTEGRATOR_SPEED * output_voltage_current;
    head_speed_current = head_speed_current + (head_speed - head_speed_current) * head_speed_tau;
    head_speed(step) = head_speed_current;

end


#
clf;
subplot (1, 2, 1);
    plot(timeline, target_position, "r",
        timeline, head_position, "g");
    title('Results');
    xlabel('timeline');
    ylabel('results');
    grid on;
subplot (1, 2, 2);
    plotyy(timeline, head_position, timeline, output_voltage);