set(fig.start,'visible','off')
z = 1;
inc = 0;
%mode = 1;
modePos = 0;
modeNeg = 0;
defaultPos = 0;
defaultNeg = 0;
%grip = 0;

if classed ~=6
    ModeCount = ModeCount+1;
else
    ModeCount = 0;
end

if classed == 1
    comm = fig.EMovement1.Value;
elseif classed == 5
    comm = fig.EMovement2.Value;
elseif classed == 2
    comm = fig.EMovement3.Value;
elseif classed == 3
    comm = fig.EMovement4.Value;
elseif classed == 4
    comm = fig.EMovement5.Value;
else
    comm = 6;
end

if comm == 1
    defaultPos = 1;
elseif comm == 2
    defaultNeg = 1;
elseif comm == 3
    if ModeCount == 1
        if mode == 3
            mode = 1;
        else
            mode = mode + 1;
        end
    end
    if mode == 1
        modelabel = 'y';
    elseif mode == 2
        modelabel = 'z';
    elseif mode == 3
        modelabel = 'grip';
    
    end
    set(fig.Mode,'String',modelabel)
elseif comm == 4
    modePos = 1;
elseif comm == 5
    modeNeg = 1;
elseif comm == 6
    defaultPos = 0;
    defaultNeg = 0;
    modePos = 0;
    modeNeg = 0;
end

if mode == 1
    if modePos == 1
        action = [0 30 0 0 0 0];
    elseif modeNeg == 1
        action = [0 -30 0 0 0 0];
    end
elseif mode == 2
    if modePos == 1
        action = [0 0 30 0 0 0];
    elseif modeNeg == 1
        action = [0 0 -30 0 0 0];
    end
elseif mode == 3
    if modePos == 1
        grip = 0;
    elseif modeNeg == 1
        grip = 1;
    end
end

if defaultPos == 1
    action = [30 0 0 0 0 0];
elseif defaultNeg == 1
    action = [-30 0 0 0 0 0];
end

if defaultPos == 0 && defaultNeg == 0 && modePos == 0 && modeNeg == 0
    action = [0 0 0 0 0 0];
end

values_to_send = [current(1:6)+action grip]
current
same = current == values_to_send;

if length(find(same==0))>0
    disp('robot')
    data_received=exchangeData(port,host,number_of_retries,samplingrate,Double2CSV(values_to_send));
end

current = values_to_send;

if current(1) > 550
    current(1) = 550;
elseif current(1) < 350
    current(1) = 350;
end

if current(2) > 200
    current(2) = 200;
elseif current(2) < -200
    current(2) = -200;
end

if current(3) > 200
    current(3) = 200;
elseif current(3) < -245
    current(3) = -245;
end

comm_mat = [defaultPos mode defaultNeg modePos modeNeg];
defaultPos = 0; defaultNeg = 0; modePos = 0; modeNeg = 0;
