본문 바로가기

Enginius/Robotics

소나 센서를 이용한 파이오니어 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));


'Enginius > Robotics' 카테고리의 다른 글

Q Learning for idiots  (0) 2015.04.30
T-RO paper submission procedure  (0) 2015.04.24
Gaussian process path  (0) 2015.01.21
Survey on people tracking for mobile robots  (0) 2015.01.19
Gaussian process path  (0) 2014.12.24