TIME_STEP = 64;


silnik1 = wb_robot_get_device('left wheel motor');
silnik2 = wb_robot_get_device('right wheel motor');

wb_motor_set_position( silnik1, inf );
wb_motor_set_position( silnik2, inf );

lazik = wb_supervisor_node_get_from_def( 'robot1' )

translacja = wb_supervisor_node_get_field( lazik, 'translation' )
rotacja = wb_supervisor_node_get_field( lazik, 'rotation' )

zegar = tic
t = toc(zegar)
v = 1
while wb_robot_step(TIME_STEP) ~= -1
dt = toc(zegar) - t
t = toc(zegar)
polozenie = wb_supervisor_field_get_sf_vec3f( translacja );
obrot = wb_supervisor_field_get_sf_rotation( rotacja );
wykres1.XData = polozenie(3)
wykres1.YData = polozenie(1)
x = polozenie(3)
y = polozenie(1)
theta = obrot(4)
[a,w] = sterowanie( t, x, y, theta, v )
v = v + a*dt;

R = 0.02;
L = 0.05;
v_12 = inv( [ R/2 R/2 ; R/L, -R/L ]) * [v;w];

wb_motor_set_velocity( silnik1, -v_12(1) );
wb_motor_set_velocity( silnik2, -v_12(2) );

drawnow;

end

function [a,w] = sterowanie( t, x, y, theta, v )
% trajektoria
xr = sin(0.1*t);
yr = cos(0.1*t);
dxr = 0.1*cos(0.1*t);
dyr = -0.1*sin(0.1*t);
ddxr = -0.01*sin(0.1*t);
ddyr = -0.01*cos(0.1*t);

 % robot:
dx = v*cos(theta);
dy = v*sin(theta);
% opis celu sterowania
e = [xr;yr] - [x;y];
de = [dxr;dyr] - [dx;dy];
dde = -10*de-25*e;
% sterowanie
A = [ cos(theta), -v*sin(theta) ; ...
sin(theta), v*cos(theta) ];
aw = inv(A)*(-dde + [ddxr;ddyr] );
a = aw(1);
w = aw(2);
end