1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
|
%
% Bras Articulé
%
function test
clc
close all
clear all
% initialisation
% STRUCT('L', longeur ,'t', angles par rapport à R0,'coord',coordonnées)
Corps(1) = struct('L', 5,'t',30*pi/180,'coord',[]);
Corps(2) = struct('L', 4,'t',Corps(1).t + 45*pi/180,'coord',[]);
Corps(3) = struct('L', 4,'t',Corps(2).t - 10*pi/180,'coord',[]);
% Mettre à jour le coordonées
update_coordinates();
% Plot
figure(1)
hold on
for v = 1:numel(Corps)
Coor = Corps(v).coord;
line(Coor(1,:),Coor(2,:),'linewidth',3,'marker','o','color','g');
end
axis equal
%-- EXTRA
% Changer repére
R = struct('Origin',[1; 2],'t',30*pi/180);
change_coordinates(R);
figure(2)
hold on
for v = 1:numel(Corps)
Coor = Corps(v).coord;
line(Coor(1,:),Coor(2,:),'linewidth',3,'marker','o','color','g');
end
axis equal
%----------------
function update_coordinates()
% Update function
if isempty(Corps)
return
end
varStart = [0 ; 0];
for u=1:numel(Corps)
Corps(u).L * [cos(Corps(u).t); sin(Corps(u).t)];
current = varStart + Corps(u).L * [cos(Corps(u).t); sin(Corps(u).t)];
Corps(u).coord = [varStart current];
varStart = current;
end
end
function change_coordinates(R)
% Update function
if isempty(Corps)
return
end
% Revenir à R0 au cas ou !!
update_coordinates();
varRotation = [cos(R.t) -sin(R.t); sin(R.t) cos(R.t)]
for u=1:numel(Corps)
Corps(u).coord = varRotation * Corps(u).coord - [R.Origin R.Origin];
end
end
end |
Partager