clc
close all
clear all

N=35;
posInit=-90;
posFinal=90;
speed=100;
speeddes=speed;

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);

%Ascending: Position
posreal(1)=readPos(H);
tic
setPosId(H,id,posFinal);
time(1)=toc;
for i=2:N
    posreal(i)=readPos(H);
    time(i)=toc;
end

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

save 2arm1massLoadedSpeed100AscendingPosition valNoSpeedChange

pause;

%Descending: Position
posreal=zeros(1,N);
pw=zeros(1,N);
v=zeros(1,N);
time=zeros(1,N);

posreal(1)=readPos(H);
tic
setPosId(H,id,posInit);
time(1)=toc;
for i=2:N
    posreal(i)=readPos(H);
    time(i)=toc;
end

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

save 2arm1massLoadedSpeed100DescendingPosition valNoSpeedChange

pause;

%Ascending: Pulse Width and Voltage
posreal=zeros(1,N);
pw=zeros(1,N);
v=zeros(1,N);
time=zeros(1,N);

tic
setPosId(H,id,posFinal);
time(1)=toc;
for i=2:N
    [pw(i),v(i)]=readPwV(H);
    time(i)=toc;
end

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

save 2arm1massLoadedSpeed100AscendingPwV valNoSpeedChange

pause;

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

tic
setPosId(H,id,posInit);
time(1)=toc;
for i=2:N
    [pw(i),v(i)]=readPwV(H);
    time(i)=toc;
end

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

save 2arm1massLoadedSpeed100DescendingPwV valNoSpeedChange

pause;

%Ascending: 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
    posreal(i)=readPos(H);
    time(i)=toc;
    [pw(i),v(i)]=readPwV(H);
end

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

save 2arm1massLoadedSpeed100AscendingBoth valNoSpeedChange

pause;

%Descending: 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,posInit);
time(1)=toc;
for i=2:N
    posreal(i)=readPos(H);
    time(i)=toc;
    [pw(i),v(i)]=readPwV(H);
end

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

save 2arm1massLoadedSpeed100DescendingBoth valNoSpeedChange

killCom(H);