clc  % Clear Command Window
clear %Remove items from workspace, freeing up system memory
close all % Remove specified figure

%%%%%%%%%%%%%%%%%%% Initial conditions %%%%%%%%%%%%%%%%%%%%%

x1(1) = 0;  % initial center position (x axis) in meters (m)
y1(1) = 0;  % initial center position (y axis) in meters (m)
phi(1)=pi/6; % initial orientation in radians

q1(1)=pi/12; % initial angular position q2 in radians
q2(1)=pi/3; % initial angular position q3 in radians

a=0.2; % distance to interest point in meters (m)
b=0.25; % height 
c=0.25;
l1=0.5;
l2=0.55;

% Geometric model

% mobile platform
x(1)=x1(1)+a*cos(phi(1));
y(1)=y1(1)+a*sin(phi(1));

% arm 
hx(1)=x(1)+l1*cos(q1(1))*cos(phi(1))+l2*cos(q1(1)+q2(1))*cos(phi(1));   % initial interest position (x axis) in meters (m)
hy(1)=y(1)+l1*cos(q1(1))*sin(phi(1))+l2*cos(q1(1)+q2(1))*sin(phi(1));   % initial interest position (y axis) in meters (m)
hz(1)=b+c+l1*sin(q1(1))+l2*sin(q1(1)+q2(1)); % initial interest position (z axis) in meters (m)


%%%%%%%%%%%%%%%%%%%%% movement of the ROBOT %%%%%%%%%%%%%%%%%%%
scene=figure;  % new figure
set(scene,'position',[250 150 980 600]); % position and size figure in the screen
axis equal; % Set axis aspect ratios
axis([-2 2 -2 2 -0.5 1.5]); % Set axis limits 
% view([135 35]); % orientation figure
% grid on; % Display axes grid lines

% MobileRobot; % Parameters of platform robot 
% M1=MobilePlot(x1(1),y1(1),phi(1)); % Plot robot in initial position x1,y1 and phi orientation
hold on
xlabel('x(m)'); ylabel('y(m)'); zlabel('z(m)'); % Label axis
camlight right % scene light

% M2=plot3(hx(1),hy(1),hz(1),'gO','LineWidth',2); % Initial point.
% 
Arm_Parameters(1); % Parameters of arm robot
M5=Arm_Plot3D(x(1),y(1),b,0,0,phi(1),q1(1),q2(1),0);% Plot robot in initial position hx,hy and phi orientation


