%--------------------------------------------------------------------------
% File:         Sim_OneLeg.m
% Author: 		Filipe Silva
% Date:			23Sept2010
% Purpose:      Kinematic and dynamic modelling/simulation of one single 
%               leg with 3dof-3D
%--------------------------------------------------------------------------
clear all
clc
SimParam
%//////////////////////////////////////////////////////////////////////////
% @MAIN_UNIT (remote computer or PC104 unit)
%//////////////////////////////////////////////////////////////////////////
% The high-level motion directives include:
% 1- Point-to-point motion planning in the joint space
% 2- Continuous motion planning in the Cartesian space
% 3- COP-based motion (on-line motion planning)
%--------------------------------------------------------------------------
% Select the 
%--------------------------------------------------------------------------
while true,

%-- Control the while cycle -----------------------------------------------
 clc
 CTRL_flag = input('Press C to continue or E to exit: ','s');
 if isempty(CTRL_flag), CTRL_flag = 'c'; end
 if CTRL_flag == 'e' | CTRL_flag == 'E', break, end
%-- Select the control mode -----------------------------------------------
 CTRL_mode = input('Input the CONTROL_mode (1, 2 or 3): ');
 if isempty(CTRL_mode), CTRL_mode = 1; end
%-- Set the target variables ---------------------------------------------- 
 XYZ_f = input('Input the target coordinates [X Y Z]: ');
 XYZ_f = XYZ_f';
 if isempty(XYZ_f), XYZ_f = [0 0 sum(L)]; end
%-- Target coordinate is inside the work volume? --------------------------
 if ( XYZ_f(1)^2+XYZ_f(2)^2+(XYZ_f(3)-l0)^2 > (l1+l2)^2 ), 
    disp('WorkSpaceOut'), continue
 else plot3(XYZ_f(1),XYZ_f(2),XYZ_f(3),'k.'), end
%-- Set the execution time ------------------------------------------------  
 Texe = input('Input the execution time: ');
 if isempty(Texe), Texe = 5; end % Default execution time
%-- Send the input parameters to the slave unit --------------------------- 
 SendData2SlaveUnit(CTRL_mode, XYZ_f, Texe);
%/////////////////////////////////////////////////////////////waitfor_REPLY

%//////////////////////////////////////////////////////////////////////////
% @SLAVE_UNIT (dsPIC)
%//////////////////////////////////////////////////////////////////////////
%--------------------------------------------------------------------------
% Move the joints using the variables: CONTROL_mode, XYZ_target, dq_max
%--------------------------------------------------------------------------
 switch CTRL_mode,
     
     case 1, 
            % qi = GetJointPositions: emulated by the GLOBAL VARIABLE qi
            qf = InvKin3dof(XYZ_f,L); % Inverse kinematics algorithm 
            JointVel = GetSyncMotion(qi,qf,Texe); % Sync if linear!
            SetJointVelocities(JointVel);
            SetJointPositions(qf); % Send the command to servos
            
     case 2,
            q = GetJointPositions; % emulates sensors using the GLOBAL qi
            XYZ_o = DirKin3dof(q,L);   % Get current hip XYZ_coordinates
            t = 0; % Timer inicialization
            while abs(t-Texe) > 1e-10,
                
                  % Open-loop motion control ------------------------------
                  % [xe,dxe,ddxe] = PolyTrajGenerator(t,Texe,[XYZ_o(1) XYZ_f(1)],[0 0],[]);
                  % [ye,dye,ddye] = PolyTrajGenerator(t,Texe,[XYZ_o(2) XYZ_f(2)],[0 0],[]);
                  % [ze,dze,ddze] = PolyTrajGenerator(t,Texe,[XYZ_o(3) XYZ_f(3)],[0 0],[]);
                  % dr = [dxe,dye,dze]'; % Reference Cartesian space vels
                  % Close-loop motion control -----------------------------                  
                  XYZ_i = DirKin3dof(q,L);% Get current hip XYZ_coordinates
                  dr_act = XYZ_f-XYZ_i;       d_act = sqrt(dr_act'*dr_act);
                  if t < 0.1*Texe, NLin = t/(0.1*Texe); else NLin = 1; end                  
                  mod_dr = NLin*d_act/(Texe-t); % Linear velocity magnitude
                  dr =mod_dr*dr_act/d_act; % Reference Cartesian space vels                  
                  hold on
                  plot3(XYZ_f(1),XYZ_f(2),XYZ_f(3),'r*', [XYZ_i(1) XYZ_i(1)+dr_act(1)],[XYZ_i(2) XYZ_i(2)+dr_act(2)],[XYZ_i(3) XYZ_i(3)+dr_act(3)],'b-.' )                  
                  % Differential inverse kinematics -----------------------
                  q = GetJointPositions; % emulates sensors using GLOBAL qi
                  JointVel = InvKinDiff3dof(q,dr,L); % New joint speed
                  SetJointVelocities(JointVel); 
                  SetJointPositions(pi*sign(JointVel)); %CORRIGIR: basta mudar velocidade
                  t = t + NUM_samples*Ts; % Current time (spent@dsPIC) 
                  %pause(0.0001)
            end
            SetJointVelocities([0 0 0]'); 
     case 3,
         
 end
    
end %----------------------------------------------------------endWHILETRUE
%--------------------------------------------------------------------------
% Exit
%--------------------------------------------------------------------------
 clc, disp('ByeBye!!...--------------------------------------------------')
 close all, return
