Skip to content
Permalink
Fetching contributors…
Cannot retrieve contributors at this time
29 lines (22 sloc) 711 Bytes
function advance(CA,CB,CC,x)
% This function predicts the target location using the
% locate target function and advances the midpoint of the formation
% towards the predicted location
global A B C rssi_a rssi_b rssi_c;
old_a = A;
old_b = B;
old_c = C;
point = locate_target(CA,CB,CC);
mid = determine_midpoint(CA,CB,CC);
yaw = determine_yaw(mid,point);
A = set_rotation(CA, yaw);
B = set_rotation(CB, yaw);
C = set_rotation(CC, yaw);
A = local_translation(A,'A',x,0);
B = local_translation(B,'B',x,0);
C = local_translation(C,'C',x,0);
hyp_a = sqrt((old_a(1,3)-A(1,3))^2+(old_a(2,3)-A(2,3))^2);
AX = cosd(yaw)*hyp_a;
AY = sind(yaw)*hyp_a;
fprintf('Translate Locations: (%d,%d)\n', AX, AY);
end
You can’t perform that action at this time.