%------------------------------------------------------------------------
% Ficheiro: robcom_test_17Dez2007.m
% Autor:    Miguel Oliveira [mriem@mec.ua.pt]
%           Vitor Santos [vsantos@mec.ua.pt]
%           Departamento de Engenharia Mecanica, Universidade de Aveiro
%           Rui Cancela (autor da robCOMM)
% Data:     17Dez07
% Funçao:   Esta demo pretende exemplificar como poderao ser usadas as
% funcionalidades da toolbox robCOMM 
%
%------------------------------------------------------------------------

clc

%robCOMM server needs this time to go back listening
pause(4);

%Define communication vars
robCOMM.port=4900;
robCOMM.ip='192.168.0.230';

%start echo on port
echotcpip('on',robCOMM.port);

%create tcpip object
robCOMM.handle = tcpip(robCOMM.ip, robCOMM.port);
set(robCOMM.handle,'Timeout',1); %1 sec timeout


%open tcpip object
fopen(robCOMM.handle);
whos robCOMM
%__________________Get current robot position____________
%send GETCRCPOS
fprintf(robCOMM.handle, 'GETCRCPOS\n');

%GETCRCPOS is repplied with
% Current position
% .001   802.031   327.443
% 65.003     -.001   180.000
% F U T, 0, 0, 0

%read text Current position
[msg, bytesread]=fgetl(robCOMM.handle);
%read xyz and convert to numbers
[msg, bytesread]=fgetl(robCOMM.handle);
[XYZ]=str2num(msg);
%read wpr and convert to numbers
[msg, bytesread]=fgetl(robCOMM.handle);
[WPR]=str2num(msg);
%read redundancy and convert to numbers
[msg, bytesread]=fgetl(robCOMM.handle);
if (msg(1)=='F') RED(1)=1; else RED(1)=0; end
if (msg(3)=='U') RED(2)=1; else RED(2)=0; end
if (msg(5)=='T') RED(3)=1; else RED(3)=0; end
[RED(4:6)]=str2num(msg(6:size(msg,2)));

%buid CRCPOS variable
CRCPOS=[XYZ WPR RED]


%__________________Move a bit more the X coord____________
newCRCPOS=[(CRCPOS(1)-20) CRCPOS(2:size(CRCPOS,2))]

Wait=0;
%send MOVTOCPOS
msg=sprintf('MOVTOCPOS\n%s\n%s\n%s\n%s\n%s\n', ...
    num2str(newCRCPOS(1:3)), ...
    num2str(newCRCPOS(4:6)), ...
    num2str(newCRCPOS(7:9)), ...
    num2str(newCRCPOS(10:12)),...
    num2str(Wait))
fprintf(robCOMM.handle, msg);

%este pause deve terminar quando o robo acabar o movimento

WaitForFanucToStop(robCOMM)
% while bytesread==0
%     [msg, bytesread]=fgetl(robCOMM.handle)
%     pause(.5);
% end

fclose(robCOMM.handle)
%stop echo on port
echotcpip('off');
delete(robCOMM.handle);
clear robCOMM;