%% Connect Matlab to Kephera robot in Webots
%
% This is a life script example how to control a robot from MATLAB by sending commands over TCP/IP, based on an example provided by
% https://www.mathworks.com/products/instrument/supported/tcp-ip.html
% actually, this script demonstrates how you could connect remotely to a Kephera robot simulated in Webots:
% https://www.cyberbotics.com/doc/guide/interfacing-webots-to-third-party-software-with-tcp-ip
%% Prerequisites
% To let this example work, we expect that you have installed Webots on your local computer (because it uses the ip of the localhost, which is always IP 127.0.0.1). Webots is available from
% https://www.cyberbotics.com/#webots
% Once installed, we expect that you load the world "khepera_tcpip.wbt", which can be found in the directory "WEBOTS_HOME/projects/robots/k-team/khepera1/worlds".
%
% This world not only loads the robot in a small worlds, but also starts a tcpip server that is waiting for your commands:
%
% This MATLAB Live script will give those commands.

% MATLAB connecting to Kephara robot simulated in Webots using TCP/IP 
% This simple code is based on the matlab_tcpip_example.m that demonstrates 
% how you can use MATLAB to exchange data with a remote application not developed in MATLAB. 
% This code example is taken from a MATLAB Digest technical article written by 
% Edward J. Mayhew from George Mason University.  In the original example
% HTTP was used as the higher-level protocol, but this example shows that you can use 
% other protocols. 
% This example requires MATLAB and the Instrument Control Toolbox. 
%

%
% On line 16, substitute "127.0.0.1" with the IP4 address if Webots is running 
% on another remote computer.

% Create TCP/IP object 't'. Specify server machine and port number. 
t = tcpip('127.0.0.1', 10020); 
% The tcpip.c code of Webots specifies that it will listen on port 10020 (line 69)

% Set size of receiving buffer, if needed. 
set(t, 'InputBufferSize', 30000); 

% Open connection to the server. 
fopen(t); 

% Now the connection is open, we can send commands. There are several commands that can be given. Run each command by calling "Run Section" in the menu above.
%
%% Change the state of the LEDS
% change the state of LED 0

% See tcpip.c line 236-249
% Transmit LED command to the server. 
fprintf(t, 'L,0,2'); 

% Pause for the communication delay, if needed. 
pause(1); 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t); 
end 

%
% change the state of LED 1

% See tcpip.c line 236-249
% Transmit LED command to the server. 
fprintf(t, 'L,1,2'); 

% Pause for the communication delay, if needed. 
pause(1) 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t); 
end 

%
%% Drive the Motors
% forward

% See tcpip.c line 230-234
% Transmit Drive command to the server. 
fprintf(t, 'D,1,1'); 

% Pause for the communication delay, if needed. 
pause(1); 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t); 
end 

%
% turn right

% See tcpip.c line 230-234
% Transmit Drive command to the server. 
fprintf(t, 'D,1,-1'); 

% Pause for the communication delay, if needed. 
pause(1); 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0)
    t.BytesAvailable;
    DataReceived = fscanf(t); 
end 

%
% turn left

% See tcpip.c line 230-234
% Transmit Drive command to the server. 
fprintf(t, 'D,-1,1'); 

% Pause for the communication delay, if needed. 
pause(1);

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t); 
end 

%
% backwards

% See tcpip.c line 230-234
% Transmit Drivecommand to the server. 
fprintf(t, 'D,-1,-1'); 

% Pause for the communication delay, if needed. 
pause(1);

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0)
    t.BytesAvailable;
    DataReceived = fscanf(t); 
end 

%
% stop

% See tcpip.c line 230-234
% Transmit Drive command to the server. 
fprintf(t, 'D,0,0'); 

% Pause for the communication delay, if needed. 
pause(1);

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t);
end 

%
%% Read the encoder values
% Note that the results are displayed at the right of the section (not in the Command Window below).

% See tcpip.c line 270-276
% Transmit command to the server. 
fprintf(t, 'H'); 

% Pause for the communication delay, if needed. 
pause(1); 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t);
end 

[E,n] = sscanf(DataReceived,'h,%d,%d',[1,2]);
if (n==2)
    answer = sprintf('Encoder values of left and right wheel are resp. %d and %d.',E(1,1),E(1,2));
else
    answer = sprintf('No correct Encoder response received: %s', E(1,1));
end
display(answer);

%
%% Read the distance sensor values
% Note that a distance of zero has to be interpreted as no obstacle detected (max range).

% See tcpip.c line 262-268
% Transmit command to the server. 
fprintf(t, 'N'); 

% Pause for the communication delay, if needed. 
pause(1); 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t);
end 

[dist,n] = sscanf(DataReceived,'n,%d,%d,%d,%d,%d,%d,%d,%d',[1,8]);
if (n==8)
    answer = sprintf('Distance measurements from left and right are resp. (%d,%d,%d,%d,%d,%d,%d,%d).',dist(1,1:8));
else
    answer = sprintf('No correct Distance response received: %s', E(1,1));
end
display(answer);

%
%% Read the ambient light sensor readings
%

% See tcpip.c line 278-284
% Transmit command to the server. 
fprintf(t, 'O'); 

% Pause for the communication delay, if needed. 
pause(1); 

% Receive lines of data from server 
while (get(t, 'BytesAvailable') > 0) 
    t.BytesAvailable;
    DataReceived = fscanf(t);
end 

[color,n] = sscanf(DataReceived,'o,%d,%d,%d,%d,%d,%d,%d,%d',[1,8]);
if (n==8)
    answer = sprintf('Ambient color measurements from left and right are resp. (%d,%d,%d,%d,%d,%d,%d,%d).',color(1,1:8));
else
    answer = sprintf('No correct Ambient response received: %s', E(1,1));
end
display(answer);

%
%% Exit
% Finally, the command to disconnect


fprintf(t, 'exit');
% Disconnect and clean up the server connection. 
fclose(t); 
delete(t); 
clear t

% Copyright 2006 - 2014 The MathWorks, Inc.