% % % Návrh úchopné hlavice manipulačního robota
% % % 
% % % Autor: Jaroslav Seifrt
% % % Vedoucí BP: doc. Ing. Petr Kolář, Ph.D.
% % % Konzultant: Ing. Michal Rytíř


%%Tento skript slouží k výpočtu uchopovací síly v závislosti na tlaku v
%%pneumotorech a na uchopovacích parametrech. Skript slouží zejména při
%%kontrole narůstající uchopovací síly.

%%Zadání parametrů uchopovaného objektu se nachází na řádku 90 - 94


clear all

%zavedení symbolických proměnných
syms RAx RAy RBx RBy RCx RCy RDx RDy REx REy RFx RFy T MG NG NH NI RHy N
syms eq1 eq2 eq3 eq4 eq5 eq6 eq7 eq8 eq9 eq10 eq11 eq12 eq13 eq14 eq15 eq16 eq17 eq18 
syms e1 fi_mez F hmotnost T fi mic mip v tlak
%% Rozměry
%zadání rozměrů mechanismu
g=9.81;
ah=17.5;
bh=14;
ch=14;
h=50 ;
l=113;
o=11.85;
q=19.45;
bd=35 ;
cd=14;
m=78.25;
b=bd-bh;

%dopočet dalších rozměrů a veličin

F=tlak*(47)/(6);
fi_=pi-fi;
p=(b^2+l^2-2*b*l*cos(fi_))^0.5;
n=p-m;

    x0k=28;
    odecet = 12;
    xc0=x0k-odecet;
    
    
     x0h=(35*1.5-14)/2;
    odecet = 12;
    xch=x0h-odecet;

%% Rovnice
%Zapúsání soustavy rovnic
eq1=RAx+RBx+RCx-N==0;
eq2=RAy+RBy+RCy+T==0;
eq3=RBy*bh+RCy*(ch+bh)+T*(ah+bh+ch)+N*h==0;

eq4=-RCx-RFx==0;
eq5=-RCy-RFy==0;
eq6=-RCy*l*cos(fi)+RCx*l*sin(fi)==0;

eq7=-RBx-REx==0;
eq8=-RBy-REy==0;    
eq9=-RBy*l*cos(fi)+RBx*l*sin(fi)==0;

eq10=-RAx-NG*sin(v)+F*cos(v)==0;
eq11=-RAy+NG*cos(v)+F*sin(v)==0;
eq12=-MG-NG*m==0;

eq13=-RDx+NG*sin(v)-F*cos(v)==0;
eq14=-RDy-F*sin(v)-NG*cos(v)==0;
eq15=MG-NG*n==0;

eq16=RDx+REx+RFx-NH-NI==0;
eq17=RDy+REy+RFy-RHy==0;
eq18=REy*bd+RFy*(bd+cd)-NH*o-NI*(o+q)-RHy*(bd+(cd/2))==0;



%% reseni
%Obecné řešení soustavy rovnic pomocí funkce solce
[N, RAx, RAy, RBx, RBy, RCx, RCy, RDx, RDy, REx, REy, RFx, RFy, MG, NG, NH, NI, RHy]=(solve( eq1, eq2, eq3, eq4, eq5, eq6, eq7, eq8, eq9, eq10, eq11, eq12, eq13, eq14, eq15, eq16, eq17, eq18, N, RAx, RAy, RBx, RBy, RCx, RCy, RDx, RDy, REx, REy, RFx, RFy, MG, NG, NH, NI, RHy ));

reseni=vpa([N, T, RAx, RAy, RBx, RBy, RCx, RCy, RDx, RDy, REx, REy, RFx, RFy, MG, NG, NH, NI, RHy]);
reseninazvy=["F" "T" "RAx" 'RAy' 'RBx' 'RBy' 'RCx' 'RCy' 'RDx' 'RDy' 'REx' 'REy' 'RFx' 'RFy' 'MG' 'NG' 'NH' 'NI' 'RHy'];

%% Řešení s konkrétními parametry uchopování

%zadání parametrů
            tvar=2; %1 pro objet tvaru koule %2 pro objekt tvaru hranol
            charakteristicky_rozmer=75; %[mm]
            hmotnost=1; %[kg]
            mi_uchop=0.55;
            tlak_=4.524956;  %bar
            
            
disp("Tvar ="); disp(tvar);
  disp("rozměr[mm] ="); disp(charakteristicky_rozmer);
   disp("Hmotnost[kg] ="); disp(hmotnost);
    disp("Uchopovací tření[-] ="); disp(mi_uchop);
    disp("tlak[bar] ="); disp(tlak_);


e1=sin(pi/2+fi_mez)/b==sin(pi/2)/l;
[fi_mez]=vpa(solve(e1,fi_mez));
fi_mez=pi-fi_mez(2);



%výpočet T a fi
if(tvar==1) %pro objet tvaru koule
    
    d=charakteristicky_rozmer;
    T_=hmotnost*g/3; %určení třecí síly
    fip=acos(-((d/2)-xc0)/l);
    
    
    if(fip<=fi_mez&&fip>0) %určení úhlu v
       vp=asin((l/p)*sin(fip));
    else
        vp=pi-asin((l/p)*sin(fip));
    end
    
    %dosazení do obecného řešení
    Nk=subs(N,v,vp);
    Nk=subs(Nk,mip,mi_uchop);
    Nk=subs(Nk,T,T_);
    Nk=subs(Nk,tlak,tlak_);
    Nk=vpa(subs(Nk,fi,fip));
     disp("N [N]= "); disp(Nk);
    
    

else if(tvar==2) %pro objekt tvaru hranol
   
    
    s=charakteristicky_rozmer;
    fip=acos(-((s/2)-xch)/l);
    
    %dopočtení úhlu v
    if(fip<=fi_mez&&fip>0)
       vp=asin((l/p)*sin(fip)); 
    else
        vp=pi-asin((l/p)*sin(fip));
    end
    
    %Určení třecí síly
    T1=hmotnost*g/2;
    T2=hmotnost*g/4;
    
    %dosazení do obecného češení
    Nk=subs(N,v,vp);
    Nk=subs(Nk,mip,mi_uchop);
    Nk=subs(Nk,tlak,tlak_);
    Nk=vpa(subs(Nk,fi,fip));
    Nk1=subs(Nk,T,T1);%Normálová síla pro prst A při zadaném tlaku a hmotnosti
    Nk2=subs(Nk,T,T2);%Normálová síla pro prst B při zadaném tlaku a hmotnosti 
    %(Tlaky se běžně při této konfiguraci liší a ze skriptu je proto požitelná vždy pouze jedna z obou hodnot, podle analyzovaného prstu)
    %Třecí síla zvětšuje uchopovací sílu N, proto se liší při stejném
    %tlaku, ale rozdílném zatížení.
     disp("N1 [N]= "); disp(Nk1);
    disp("N2 [N]= "); disp(Nk2);
        
    
    end
end


