%% Anna Findejsová, bakalářská práce: Rukavice se senzory ohybu, 2017
% Vizualizace ohybu ukazováku 

%% 1) Model ruky - nejprve je pomocí funkce robot vytvořeno pět samostatných mechanismů prstů

q1 = -pi/8; q2 = pi/4; q3 = pi/4; q4 = pi/4;
finger1 = robot([dh(pi,0,0,q1); dh(-pi/2,0.015,0,q2); dh(0,0.05,0,q3);...
 dh(0,0.04,0,q4)], ['r', 'r', 'r', 'r'],...
 'RobotBase', rp2t(roty(pi/2),[0.04;0.03;-0.015]),...
 'RobotGrasper', [0.03; 0; 0], 'RobotGrasperSize', 0,...
 'RobotCylinderRadius', 0.012); % palec
q1 = 0; q2 = -pi/16; q3 = -pi/8; q4 = -pi/8;
finger2 = robot([dh(pi,0,0,q1); dh(-pi/2,0,0,q2); dh(0,0.05,0,q3);...
 dh(0,0.03,0,q4)], ['r', 'r', 'r', 'r'],...
 'RobotBase', [0.095;0.022;0],...
 'RobotGrasper', [0.025; 0; 0], 'RobotGrasperSize', 0,...
 'RobotCylinderRadius', 0.01); % ukazovák
finger3 = robot([dh(pi,0,0,q1); dh(-pi/2,0,0,q2); dh(0,0.055,0,q3);...
 dh(0,0.035,0,q4)], ['r', 'r', 'r', 'r'],...
 'RobotBase', [0.095;0;0],...
 'RobotGrasper', [0.025; 0; 0], 'RobotGrasperSize', 0,...
 'RobotCylinderRadius', 0.01); % prostředník
finger4 = robot([dh(pi,0,0,q1); dh(-pi/2,0,0,q2); dh(0,0.05,0,q3);...
 dh(0,0.03,0,q4)], ['r', 'r', 'r', 'r'],...
 'RobotBase', [0.09;-0.02;0],...
 'RobotGrasper', [0.025; 0; 0], 'RobotGrasperSize', 0,...
 'RobotCylinderRadius', 0.009); % prsteník
finger5 = robot([dh(pi,0,0,q1); dh(-pi/2,0,0,q2); dh(0,0.04,0,q3);...
 dh(0,0.025,0,q4)], ['r', 'r', 'r', 'r'],...
 'RobotBase', [0.085;-0.038;0],...
 'RobotGrasper', [0.023; 0; 0], 'RobotGrasperSize', 0,...
 'RobotCylinderRadius', 0.008); % malík

%% Vytvoření komplexního mechanismu ruky
hand = robot_complex(finger1, finger2, finger3, finger4, finger5, ...
 'RobotBase', [0;0;0], ...
 'RobotModel', 'config', ...
 'RobotCylinderRadius', 0.01); 

%% Nastavení rozsahu pohybů pro jednotlivé prsty 1-5
robot_set(finger1, 'RobotRange', [-pi/2, 0; 0, pi/2; 0, pi/2; 0, pi/2]); 
robot_set(finger2, 'RobotRange', [-pi/9, 0; -pi/2, 0; -pi/2, 0; -pi/2, 0]);
robot_set(finger3, 'RobotRange', [-pi/18, pi/18; -pi/2, 0; -pi/2, 0; -pi/2, 0]);
robot_set(finger4, 'RobotRange', [0, pi/9; -pi/2, 0; -pi/2, 0; -pi/2, 0]);
robot_set(finger5, 'RobotRange', [0, pi/5; -pi/2, 0; -pi/2, 0; -pi/2, 0]);
axis ([-0.01 0.01 -0.01 0.01 -0.01 0.01]); % rozsah os zobrazované 3D oblasti
axis equal;
robot_plot(hand,'dmesh');
hold on

%% Zahájení komunikace s vývojovou deskou Arduino UNO, inicializace vektorů a počátečních hodnot
a = arduino(); 
t = 1;
interval = 100;

x = zeros(1,(length(interval))); % ukazovák
x(1) = 2.5; % průměrná hodnota při nataženém prstu

pause;
%% Načítání signálu z analogového pinů Arduina 'A1' - ukazovák

while (t < interval)
    forefinger = readVoltage(a,'A1');
     x = [x, forefinger];
     
 %% Podmínky pro vizualizaci ohybu ukazováku a vizualiace dle vstupních hodnot
     
     if x(t) >= 2.5 || x(t) == 0 
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q2 = [linspace(0,0,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q4 = [linspace(pi/9,pi/9,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q5 = [linspace(pi/5,pi/5,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);

     elseif x(t) >= 2.25 && x(t) < 2.5 
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q2 = [linspace(0,-pi/12,10); linspace(0,-pi/12,10); linspace(0,-pi/12,10); linspace(0,-pi/12,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q4 = [linspace(pi/9,pi/9,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q5 = [linspace(pi/5,pi/5,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);
        
     elseif x(t) >= 1.75 && x(t) < 2.25 
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q2 = [linspace(0,-pi/9,10); linspace(0,-pi/6,10); linspace(0,-pi/6,10); linspace(0,-pi/6,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q4 = [linspace(pi/9,pi/9,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q5 = [linspace(pi/5,pi/5,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);

     elseif x(t) >=1.6 && x(t) < 1.75 
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q2 = [linspace(0,-pi/9,10); linspace(0,-pi/4,10); linspace(0,-pi/4,10); linspace(0,-pi/4,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q4 = [linspace(pi/9,pi/9,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q5 = [linspace(pi/5,pi/5,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);
        
     elseif x(t) >= 1.48 && x(t) < 1.6 
        q1 = [linspace(-pi/2,-pi/2,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        q2 = [linspace(0,-pi/9,15); linspace(0,-pi/2,15); linspace(0,-pi/2,15); linspace(0,-pi/2,15)];
        q3 = [linspace(pi/18,pi/18,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        q4 = [linspace(pi/9,pi/9,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        q5 = [linspace(pi/5,pi/5,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);
        
     elseif x(t) < 1.48 && x(t) > 0
        q1 = [linspace(-pi/2,-pi/2,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        q2 = [linspace(-pi/9,-pi/9,15); linspace(-pi/2,-pi/2,15); linspace(-pi/2,-pi/2,15); linspace(-pi/2,-pi/2,15)];
        q3 = [linspace(pi/18,pi/18,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        q4 = [linspace(pi/9,pi/9,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        q5 = [linspace(pi/5,pi/5,15); linspace(0,0,15); linspace(0,0,15); linspace(0,0,15)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);
        
        else
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q2 = [linspace(0,0,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q4 = [linspace(pi/9,pi/9,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        q5 = [linspace(pi/5,pi/5,10); linspace(0,0,10); linspace(0,0,10); linspace(0,0,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0);
     end

drawnow;  
t = t+1;

end