clc
close all
clear all

N=1000;
posInit=-90;
posFinal=90;
nconst=10;
var=ceil((rand(1,N)*2-1)*100);

H=initCom;
setPosAll(H,posInit);
[version,id]=readVersionId(H);

speed=50;
setSpeedReadPos(H,id,speed);
setPosId(H,id,posInit);
pause;

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

%Position
flag=0;
i=1;
posreal(1)=readPos(H);
speeddes(1)=speed;
[pw(1),v(1)]=readPwV(H);
tic
setPosId(H,id,posFinal);
time(1)=toc;
i=i+1;
for i=2:N
    if(mod(i,nconst)==0 || flag==1)
        speed=speed+var(i);
        if(speed>100)
            speed=100;
        else if(speed<1)
                speed=1;
            end
        end
        posreal(i)=setSpeedReadPos(H,id,speed);
        flag=0;
    else
        posreal(i)=readPos(H);
    end
    
    time(i)=toc;
%    [pw(i),v(i)]=readPwV(H);
    speeddes(i)=speed;
    
    if(posreal(i)>85)
        setPosId(H,id,posInit);
        flag=1;
    else if(posreal(i)<-85)
        setPosId(H,id,posFinal);
        flag=1;
        end
    end
    i=i+1;
end
pw=zeros(1,length(time));
v=zeros(1,length(time));

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

killCom(H);

valSpeedChange=valSpeedChange0;
save .\7.4V\noLoad\SpeedChange\PositionOrder10 valSpeedChange