function CP=getcrcpos(fanuc)

%__________________Get current robot position____________
%send GETCRCPOS
fprintf(fanuc, '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(fanuc);
%read xyz and convert to numbers
[msg, bytesread]=fgetl(fanuc);
[XYZ]=str2num(msg);
%read wpr and convert to numbers
[msg, bytesread]=fgetl(fanuc);
[WPR]=str2num(msg);
%read redundancy and convert to numbers
[msg, bytesread]=fgetl(fanuc);

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

WPR(1) = degtorad(WPR(1));
WPR(2) = degtorad(WPR(2));
WPR(3) = degtorad(WPR(3));

%buid CRCPOS variable
CP=[XYZ WPR RED];

%Clear the trash
fgetl(fanuc);
fgetl(fanuc);
fgetl(fanuc);
