%-- Funçao Homing
%------------------------------------------------------------------------
StepSize = pi/180;
ANG = [ 0:-StepSize:-pi/6 -pi/6:StepSize:pi/6 pi/6:-StepSize:0
        0:-StepSize:-pi/6 -pi/6:StepSize:pi/6 pi/6:-StepSize:0
        0:-StepSize:-pi/6 -pi/6:StepSize:pi/6 pi/6:-StepSize:0
        0:-StepSize:-pi/6 -pi/6:StepSize:pi/6 pi/6:-StepSize:0
        0:-StepSize:-pi/6 -pi/6:StepSize:pi/6 pi/6:-StepSize:0];

Junta = ['Ankle Joint (lateral)'; 'Ankle Joint (sagital)'
         'Knee Joint  (sagital)'; 
         'Hip Joint   (sagital)'; 'Hip Joint   (lateral)' ];
for i=1:size(ANG,1),
    disp(sprintf(' %s ', Junta(i,:)) )
    for j=1:size(ANG,2),
        switch i,
            case 1, ang1 = ANG(i,j); ang2 = 0; ang3 = 0; ang4 = 0; ang5 = 0;
            case 2, ang1 = 0; ang2 = ANG(i,j); ang3 = 0; ang4 = 0; ang5 = 0;
            case 3, ang1 = 0; ang2 = 0; ang3 = ANG(i,j); ang4 = 0; ang5 = 0;
            case 4, ang1 = 0; ang2 = 0; ang3 = 0; ang4 = ANG(i,j); ang5 = 0;
            case 5, ang1 = 0; ang2 = 0; ang3 = 0; ang4 = 0; ang5 = ANG(i,j);
        end
        q = [ang1 ang2 ang3 ang4 ang5];
        P = CinDir5dof(ang1,ang2,ang3,ang4,ang5,l0,l1,l2,l3,l4,l5);
        Rasto = [0; 0; 0];
        [C1,C2,C3,C4,C5] = CoordJuntas( ang1,ang2,ang3,ang4,ang5,l0,l1,l2,l3,l4,l5 );
        COG = RobCOG( q,l0,l1,l2,l3,l4,l5,r0,r1,r2,r3,r4,r5,m0,m1,m2,m3,m4,m5 );
        DesenharRobo( P, C1, C2, C3, C4, C5, COG, Rasto, AZ, EL )
        pause(0.001)
    end
    pause(2)
end