%--------------------------------------------------------------------------
% File:         Sim_OneLeg.m
% Author: 		Filipe Silva
% Date:			23Sept2010
% Purpose:      Kinematic and dynamic modelling/simulation of one single 
%               leg with 3dof-3D
%--------------------------------------------------------------------------
clear all
close 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,
            XYZdes = [];
            q = GetJointPositions; % emulates sensors using the GLOBAL qi
            XYZ_o = DirKin3dof(q,L);   % Get current hip XYZ_coordinates
            t = 0; % Timer inicialization
            VEL_mode = 2; % 1: Target-based velocity planning -------------
                          % 2: Position-tracking-error planning -----------
            while abs(t-Texe) > 1e-10,
                
                  switch VEL_mode,
                  case 1, % Target-based motion planning ------------------                         
                   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-.' )
                  case 2, % Position-tracking-error planning --------------
                   [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],[]);
                   XYZ_i = DirKin3dof(q,L);% Get current hip XYZ_coordinates   
                   XYZdes = [XYZdes XYZ_i];
                   Kx = 10; Ky = 10; Kz = 10;
                   dr = [Kx*(xe-XYZ_i(1)) Ky*(ye-XYZ_i(2)) Kz*(ze-XYZ_i(3))]';
                  end                      
                  % 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) 
                   t; %pause(0.0001)
            end
            SetJointVelocities([0 0 0]'); 
            t = 0:Ts:(size(ANG,2)-1)*Ts;
            PlotGraphics( t,ANG,dANG,T,GRF,COP,COG );
            [xdes,dxe,ddxe] = PolyTrajGenerator(0:Ts:Texe,Texe,[XYZ_o(1) XYZ_f(1)],[0 0],[0 0]);
            [ydes,dxe,ddxe] = PolyTrajGenerator(0:Ts:Texe,Texe,[XYZ_o(2) XYZ_f(2)],[0 0],[0 0]);
            [zdes,dxe,ddxe] = PolyTrajGenerator(0:Ts:Texe,Texe,[XYZ_o(3) XYZ_f(3)],[0 0],[0 0]);
            figure, subplot(3,1,1), plot(t(1:NUM_samples:end),XYZdes(1,:),'r', 0:Ts:Texe,xdes,'b')
                    xlabel('Time (s)'), ylabel('Xe (m)'), legend('Actual','Reference')
                    subplot(3,1,2), plot(t(1:NUM_samples:end),XYZdes(2,:),'r', 0:Ts:Texe,ydes,'b')
                    xlabel('Time (s)'), ylabel('Ye (m)'), legend('Actual','Reference')
                    subplot(3,1,3), plot(t(1:NUM_samples:end),XYZdes(3,:),'r', 0:Ts:Texe,zdes,'b') 
                    xlabel('Time (s)'), ylabel('Ze (m)'), legend('Actual','Reference')
            I1_sqbin = diag(T*T')/Texe
            I2_total = trace(T*T')/Texe
            pause, close(2:6)
     case 3,
            q = GetJointPositions; % emulates sensors using the GLOBAL qi
            COG_o = RobCOG( q,L,R,M ); % Estimate the COG (only COGz!)
            qf = InvKin3dof(XYZ_f,L); % Inverse kinematics algorithm 
            COG_f = RobCOG( qf,L,R,M ); % Estimate the COG (only COGz!)
            A_i = zeros(3,3); % Initial linear CM accelerations (SIMULATION!)
            [M_01_i,T] = InvDyn3dof( A_i,q,L,R,M,go ); % USED IN SIMULATION!
            t = 0; % Timer inicialization
            while abs(t-Texe) > 1e-10,
                  % Read force sensors and estimate the actual COP -------- 
                  GRF_i = RobGRF( A_i,M,go );   % Read force sensors
                  COP_i = RobCOP( M_01_i,GRF_i,m0,CM0x,CM0y,l0,go ); % Compute COP 
                  q = GetJointPositions; % emulates sensors using GLOBAL qi
                  COG_i = RobCOG( q,L,R,M ); % Estimate the COG (only COGz!)
                  % Reference variables: desired COP & COGz ---------------
                  [COPref(1),dxe,ddxe] = PolyTrajGenerator(t,Texe,[COG_o(1) COG_f(1)],[0 0],[0 0]);
                  [COPref(2),dye,ddye] = PolyTrajGenerator(t,Texe,[COG_o(2) COG_f(2)],[0 0],[0 0]);
                  [COGref(3),dze,ddze] = PolyTrajGenerator(t,Texe,[COG_o(3) COG_f(3)],[0 0],[0 0]);
                  % Calculate the joint velocities as a control problem ---
                  Kx = 12.0; Ky = 12.0; Kz = 7.0;
                  dr = [Kx*(COPref(1)-COP_i(1)) Ky*(COPref(2)-COP_i(2))...
                                                Kz*(COGref(3)-COG_i(3))]';
                  t                                        
                  % Differential inverse kinematics -----------------------                  
                  JointVel = InvKinDiff3dof_COG( q,dr,L,R,M ); % New joint speed
                  %JointVel
                  SetJointVelocities(JointVel);
                  SetJointPositions(pi*sign(JointVel)); %CORRIGIR: basta mudar velocidade
                  t = t + NUM_samples*Ts; % Current time (spent@dsPIC) 
                  %pause
            end
            t = 0:Ts:(size(ANG,2)-1)*Ts;
            PlotGraphics( t,ANG,dANG,T,GRF,COP,COG );
            [COPxdes,dxe,ddxe] = PolyTrajGenerator(0:Ts:Texe,Texe,[COG_o(1) COG_f(1)],[0 0],[0 0]);
            [COPydes,dxe,ddxe] = PolyTrajGenerator(0:Ts:Texe,Texe,[COG_o(2) COG_f(2)],[0 0],[0 0]);
            [COGzdes,dxe,ddxe] = PolyTrajGenerator(0:Ts:Texe,Texe,[COG_o(3) COG_f(3)],[0 0],[0 0]);
            figure, subplot(3,1,1), plot(t,COP(1,:),'r', 0:Ts:Texe,COPxdes,'b')
                    xlabel('Time (s)'), ylabel('COPx (m)'), legend('Actual','Reference')
                    subplot(3,1,2), plot(t,COP(2,:),'r', 0:Ts:Texe,COPydes,'b')
                    xlabel('Time (s)'), ylabel('COPy (m)'), legend('Actual','Reference')
                    subplot(3,1,3), plot(t,COG(3,:),'r', 0:Ts:Texe,COGzdes,'b')
                    xlabel('Time (s)'), ylabel('COGz (m)'), legend('Actual','Reference')
            I1_sqbin = diag(T*T')/Texe
            I2_total = trace(T*T')/Texe
            pause, close(2:6)
 end
    
end %----------------------------------------------------------endWHILETRUE
%--------------------------------------------------------------------------
% Exit
%--------------------------------------------------------------------------
 clc, disp('ByeBye!!...--------------------------------------------------')
 close all, return
