소나 센서를 이용한 파이오니어 3DX 로봇 제어

function sonar_ctrl = getSonarControl(robot_struct, min_dist_threshold)

sonar_ctrl = [0 0];

sd = robot_struct.rfs_result;

[min_sonar_dist, min_sonar_idx] = min(sd); 

% 일단 최소 거리가 min_dist_threshold (1200 mm) 보다는 작아야 처리를 한다. 

if min_sonar_dist > min_dist_threshold

    return;

end

sonar_ctrl(1) = robot_struct.default_u(1); % 기본 직진 속도 

rel_idx = min_sonar_idx - 4.5;

sgn_rel_idx = sign(rel_idx);

% dist threshold 

dist_th1 = 1200;

dist_th2 = 900;

dist_th3 = 600;

% vel limit

vmin = -100;

vmax = 250;

if min(sd) < dist_th3

    sonar_ctrl(1) = 0;

    sonar_ctrl(2) = -35*sgn_rel_idx;

elseif min(sd(2:7)) < dist_th2

    % 전방 여섯 개의 sonar 센서에 대해서 

     sonar_ctrl(1) = 100;

     sonar_ctrl(2) = -25*sgn_rel_idx;

elseif min(sd(3:6)) < dist_th1

    % 전방 네 개의 sonar 센서에 대해서만 

    sonar_ctrl(1) = val_minamx(min(sd)-dist_th1+100, vmin, vmax);

    sonar_ctrl(2) = -20*sgn_rel_idx;

else

    

end

%%

function ret = val_minamx(val, min_val, max_val)

ret = max(min_val, min(val, max_val));