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

%% 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]);

%% Zobrazení modelu ruky
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
y = zeros(1,(length(interval))); % prostředník
y(1) = 1.25;
z = zeros(1,(length(interval))); % palec
z(1) = 1.25;
p = zeros(1,(length(interval))); % prsteník
p(1) = 2.5;
s = zeros(1,(length(interval))); %malík
s(1) = 1.25;

pause; 
%% Načítání signálu z jednotlivých analogových pinů Arduina v pořadí: ukazovák, prostředník, palec, prsteník, malík

while (t < interval)
    forefinger = readVoltage(a,'A1'); 
    x = [x, forefinger]; % dlouhý senzor
    middle_finger = readVoltage(a,'A2');
    y = [y, middle_finger]; % krátký senzor
    thumb = readVoltage(a,'A0');
    z = [z, thumb]; % krátký senzor
    ring_finger = readVoltage(a,'A3');
    p = [p, ring_finger]; % dlouhý senzor
    pinkie = readVoltage(a,'A4');
    s = [s, pinkie];
    
 %% Podmínky pro vizualizaci ohybu prstů a vizualiace dle vstupních hodnot
 
    if x(t) <2  && y(t) < 1.1 && z(t) < 1 && p(t) < 2 && s(t) < 1 
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,pi/2,10); linspace(0,pi/2,10); linspace(0,pi/2,10)];
        q2 = [linspace(0,-pi/9,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q4 = [linspace(pi/9,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q5 = [linspace(pi/5,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0); % hýbe se 5 prstů
    elseif x(t) <2  && y(t) < 1.1 && z(t) >= 1 && p(t) < 2 && s(t) < 1 
        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/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q4 = [linspace(pi/9,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q5 = [linspace(pi/5,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0); % hýbou se 4 prsty
    elseif x(t) >=2  && y(t) < 1.1 && z(t) >= 1 && p(t) < 2 && s(t) < 1 
        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,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q4 = [linspace(pi/9,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q5 = [linspace(pi/5,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0); % hýbou se 3 prsty
    elseif x(t) < 2  && y(t) < 1.1 && z(t) >= 1 && p(t) >= 1.8 && s(t) >= 1 
        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/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        q3 = [linspace(pi/18,pi/18,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,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); % hýbou se 2 prsty - ukazovák + prostředník
    elseif x(t) < 2  && y(t) >= 1  && z(t) >= 1 && p(t) >= 2 && s(t) >= 1 
        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/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,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); % hýbe se jen ukazovák
    elseif y(t) < 1.1 && x(t) >= 2 && z(t) >= 1 && p(t) >= 2 && s(t) >= 1 
        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,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,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);% hýbe se jen prostředník
    elseif p(t) < 2.2 && x(t) >= 2 && z(t) >= 1 && y(t) >= 1.1 && s(t) >= 1 
        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,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,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); % hýbe se jen prsteník
    elseif z(t) < 1 && x(t) >= 2 && y(t) >= 1.1 && s(t) >= 1 && p(t) >= 2  
        q1 = [linspace(-pi/2,-pi/2,10); linspace(0,pi/2,10); linspace(0,pi/2,10); linspace(0,pi/2,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); % hýbe se jen palec
    elseif s(t) < 1 && x(t) >= 2 && y(t) >= 1.1 && z(t) >= 1 && p(t) >= 2 
        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,0,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10); linspace(0,-pi/2,10)];
        r = robot_motion(hand, [q1; q2; q3; q4; q5], 0); % hýbe se jen malík
    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); % žádný pohyb
     end
    
    drawnow; 
    t = t+1;
end

