close all
clear 
clc
%% Load STL

base = stlRead('baseRobotUniciclo.stl'); % Export in meters and binary format (No ASCII)
wheel = stlRead('wheelRobotUniciclo.stl'); % Export in meters and binary format (No ASCII)
motor = stlRead('motorRobotUniciclo.stl'); % Export in meters and binary format (No ASCII)
hopps = stlRead('hoppsWheelRobotUniciclo.stl'); % Export in meters and binary format (No ASCII)
castor = stlRead('castorWheelRobotUniciclo.stl'); % Export in meters and binary format (No ASCII)

save('robotPatch.mat');

%% Stroboscopic movement of the ROBOT
scene=figure;  % new figure
tam=get(0,'ScreenSize');
set(scene,'position',tam); % position and size figure in the screen
axis equal; % Set axis aspect ratios
axis([-1 1 -1 1 -0 1]); % Set axis limits 
% view([135 35]); % orientation figure
grid on; % Display axes grid lines
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')

% Add a camera light, and tone down the specular highlighting
camlight('headlight');
material('dull');

x = 0;
y = 0;
z = 0;

phi = 90*(pi/180); % angle z axis [rad]
thetha = 45*(pi/180); % angle y axis [rad]
psi = 0*(pi/180); % angle x axis [rad]



scaleRobot = 2;
% Move robot
base.vertices(:,1)=base.vertices(:,1)*scaleRobot+x; %  translation x axis
base.vertices(:,2)=base.vertices(:,2)*scaleRobot+y; %  translation y axis
base.vertices(:,3)=base.vertices(:,3)*scaleRobot+z; %  translation z axis


% Rotate Robot
Rz=[cos(phi), -sin(phi) 0; sin(phi) cos(phi) 0; 0 0 1]; %Matrix Rotation z axis
Ry=[cos(thetha), 0, sin(thetha); 0, 1, 0;-sin(thetha), 0, cos(thetha)]; %Matrix Rotation y axis
Rx=[1, 0, 0;0, cos(psi), -sin(psi);0, sin(psi) cos(psi)]; %Matrix Rotation x axis


robotRotate=Rz*Ry*Rx*base.vertices'; % Consider order of Rotation Matrix 
     
patch('Faces',base.faces,'Vertices',robotRotate','FaceColor',"r",'EdgeColor','none')


