gistfile1.txt

%%%%    %%%%
function [ Lu, Ll, thata_max_min ] = fn_find_Aui_Ali (motor_angle, b_size, e_size, Wr, Wp)
r_ws = Wr;
xs =    Wp(1);
ys =    Wp(2);
zs =    Wp(3);
r_b = b_size-e_size;
gramma = motor_angle;
%%  thata are angle from minimum maximan
thata = atan2(zs,(-r_b+xs*cosd(gramma)+ys*sind(gramma))) /pi*180
%% find minimum l1 
oa =[r_b*cosd(gramma); r_b*sind(gramma); 0];
oc =[xs; ys ; zs];
u_victor = [cosd(thata)*cosd(gramma); cosd(thata)*sind(gramma); sind(thata)];
o = oa-oc;
o2= o.^2;
u = u_victor.*o;
a = 1;
b = 2*(u(1)+u(2)+u(3));
c =   o2(1)+o2(2)+o2(3);
syms ll
Lu= double(abs(solve((ll^2*a-ll*b+c)^0.5-(ll^2*a+ll*b+c)^0.5 == 2*r_ws)));
clear ll
Ll= abs((Lu^2*a-Lu*b+c)^0.5-r_ws);
thata_max_min = [-thata-180, -thata] ;
end
%%%%    %%%%