clc
close all
clear all

N=100;
speed=25;

posInit=-90;
posFinal=90;
speeddes=speed;

H=initCom;

setPosAll(H,posInit);
[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
valNoSpeedChange0.posreal=posreal;
valNoSpeedChange0.pw=pw;
valNoSpeedChange0.v=v;
valNoSpeedChange0.time=time;
valNoSpeedChange0.speeddes=speeddes;

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
valNoSpeedChange1.posreal=posreal;
valNoSpeedChange1.pw=pw;
valNoSpeedChange1.v=v;
valNoSpeedChange1.time=time;
valNoSpeedChange1.speeddes=speeddes;

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
valNoSpeedChange2.posreal=posreal;
valNoSpeedChange2.pw=pw;
valNoSpeedChange2.v=v;
valNoSpeedChange2.time=time;
valNoSpeedChange2.speeddes=speeddes;

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
valNoSpeedChange3.posreal=posreal;
valNoSpeedChange3.pw=pw;
valNoSpeedChange3.v=v;
valNoSpeedChange3.time=time;
valNoSpeedChange3.speeddes=speeddes;

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
valNoSpeedChange4.posreal=posreal;
valNoSpeedChange4.pw=pw;
valNoSpeedChange4.v=v;
valNoSpeedChange4.time=time;
valNoSpeedChange4.speeddes=speeddes;

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
valNoSpeedChange5.posreal=posreal;
valNoSpeedChange5.pw=pw;
valNoSpeedChange5.v=v;
valNoSpeedChange5.time=time;
valNoSpeedChange5.speeddes=speeddes;

killCom(H);

valNoSpeedChange=valNoSpeedChange0;
save P:\2010\Matlab\HSR-5980SG\6.0V\1armLoaded\noSpeedChange\Speed25\Position_Ascending valNoSpeedChange
valNoSpeedChange=valNoSpeedChange1;
save P:\2010\Matlab\HSR-5980SG\6.0V\1armLoaded\noSpeedChange\Speed25\Position_Descending valNoSpeedChange
valNoSpeedChange=valNoSpeedChange2;
save P:\2010\Matlab\HSR-5980SG\6.0V\1armLoaded\noSpeedChange\Speed25\PwV_Ascending valNoSpeedChange
valNoSpeedChange=valNoSpeedChange3;
save P:\2010\Matlab\HSR-5980SG\6.0V\1armLoaded\noSpeedChange\Speed25\PwV_Descending valNoSpeedChange
valNoSpeedChange=valNoSpeedChange4;
save P:\2010\Matlab\HSR-5980SG\6.0V\1armLoaded\noSpeedChange\Speed25\All_Ascending valNoSpeedChange
valNoSpeedChange=valNoSpeedChange5;
save P:\2010\Matlab\HSR-5980SG\6.0V\1armLoaded\noSpeedChange\Speed25\All_Descending valNoSpeedChange
