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
소나 센서를 이용한 파이오니어 3DX 로봇 제어  (0) 2015.02.26
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
« PREV : 1 : ··· : 120 : 121 : 122 : 123 : 124 : 125 : 126 : 127 : 128 : ··· : 587 : NEXT »