clc
close all
clear all

N=300;
posInit=-90;
posFinal=90;
speed=5;
tol=5;

H=initCom;

[version,id]=readVersionId(H);
setSpeedReadPos(H,id,speed);
setPosId(H,id,posInit);

pause;

posreal=zeros(1,N);
pw=zeros(1,N);
v=zeros(1,N);
time=zeros(1,N);
speeddes=zeros(1,N);

%Position
posreal(1)=readPos(H);
speeddes(1)=speed;
tic
setPosId(H,id,posFinal);
time(1)=toc;
for i=2:N
    if(mod(i,20)==0)
        if(speed>95 || speed<5)
            tol=-tol;
        end
        speed=speed+tol;
        if(speed>100)
            speed=100;
        else if(speed<1)
                speed=1;
            end
        end
        posreal(i)=setSpeedReadPos(H,id,speed);
    else
        posreal(i)=readPos(H);
    end
    time(i)=toc;
    
    if(posreal(i)>85)
        setPosId(H,id,posInit);
    else if(posreal(i)<-85)
        setPosId(H,id,posFinal);
        end
    end
    speeddes(i)=speed;
end

%Saving variables/results
valSpeedChange.posreal=posreal;
valSpeedChange.pw=pw;
valSpeedChange.v=v;
valSpeedChange.time=time;
valSpeedChange.speeddes=speeddes;

save noLoadSpeedChange5Position valSpeedChange

setPosId(H,id,posInit);
pause;

%Position, Pulse Width & Voltage
posreal=zeros(1,N);
pw=zeros(1,N);
v=zeros(1,N);
time=zeros(1,N);

posreal(1)=readPos(H);
tic
setPosId(H,id,posFinal);
time(1)=toc;
for i=2:N
    if(mod(i,20)==0)
        if(speed>95 || speed<5)
            tol=-tol;
        end
        speed=speed+tol;
        if(speed>100)
            speed=100;
        else if(speed<1)
                speed=1;
            end
        end
        posreal(i)=setSpeedReadPos(H,id,speed);
    else
        posreal(i)=readPos(H);
    end
    time(i)=toc;
    [pw(i),v(i)]=readPwV(H);
    
    if(posreal(i)>85)
        setPosId(H,id,posInit);
    else if(posreal(i)<-85)
        setPosId(H,id,posFinal);
        end
    end
end

%Saving variables/results
valSpeedChange.posreal=posreal;
valSpeedChange.pw=pw;
valSpeedChange.v=v;
valSpeedChange.time=time;
valSpeedChange.speeddes=speeddes;

save noLoadSpeedChange5Both valSpeedChange

killCom(H);