Distance measurement algorithm of the four ultrasonic sensors array % Data availability in the communication channel If data available continue; end; If not, wait for more data. % Data validity is expressed through the condition that the reading should be between 10 cm and 1000 cm If data < 10 || data > 1000 then data valid, continue; end;
% Addition of channel selected as they are changing since the system is using 4 channels (4 ultrasonic sensors) as organized by a multiplexer counter (channel_ID) = counter (channel_ID) + 1; % Local reflection point position where only the Z component (depth) is read while X and Y (platform displacements) are set to zeros here x = zeros(size(d)); y = zeros(size(d)); z = (0.5*d-p./(2*d)); where d = ultrasonic sensor distance p = coordinates position of the ultrasonic sensor in the array with respect to the active ultrasonic sensor |