% Delete Opened Ports in MATLAB
clear all;
clc;

delete(instrfind)

ser = serial('COM3', 'BaudRate',115200, 'Timeout',10);
syms right;
syms left;
syms stan;
stan = 1;
fopen(ser);
while true
    d = fscanf(ser, '%d %d %d');
    value1 = d(1,:);
    value2 = d(2,:);
    value3 = d(3,:);

    fprintf("%d\t%d\t%d\r\n", value1,value3,stan);


switch stan
    case 1
        if(value1<600 && value3>600)
            stan = 2;
        elseif(value1>600 && value3<600)
            stan = 4;
        end
    case 2
        if(value1>600 && value3<600)
            stan = 3;
        end
    case 3
        fprintf("z prawej");
        if(value1>600 && value3>600)
            stan = 1;
        end
    case 4
        if(value1<600 && value3>600)
            stan = 5;
        end
    case 5
        fprintf("z lewej");
        if(value1>600 && value3>600)
            stan = 1;
        end
end




    plot(d, 'LineWidth',2,'Color',[0,0,1.0])
       ylim([0,4000])

      drawnow
end


    fclose(ser);