MLscripts/ObstacleAvoidace.m at master · robocup-ssl-russia/MLscripts · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
% (x,y) - robot's coordinates, (ox,oy) - obstacle's center, ro - radius of the obstacle,
%m - radius of the robot, theta_d - robot's direction
function rul = ObstacleAvoidace( x, y, ox, oy, ro, m, theta_d )
coef1 = 15;
% distance between robot and obstacle
dist = sqrt((ox-x)*(ox-x) + (y-oy)*(y-oy));
length = fabs( (ox-x)*sin(theta_d) + (y-oy)*cos(theta_d) );
angle = atan2( oy-y, ox-x );
diff_angle = theta_d - angle;

%while (diff_angle > PI)
%    diff_angle = diff_angle - 2.*PI;
%end
%while( diff_angle < -PI )
%    diff_angle = diff_angle + 2.*PI;
%end

if (length < ro+m) && (fabs(diff_angle )< PI/2)

	if dist <= ro
        theta_d_new = angle - PI;
    else

    if dist <= ro+m
		% modify theta_d to avoid it with CW direction
		if diff_angle > 0.

			%make smooth transition near the obstacle
			%boundary
			tmp_x = ( (dist-ro)*cos(angle-1.5*PI) + (ro+m-dist)*cos(angle-PI)) / m;
			tmp_y = ( (dist-ro)*sin(angle-1.5*PI) + (ro+m-dist)*sin(angle-PI)) / m;
			theta_d_new = atan2( tmp_y, tmp_x );

		% modify theta_d to avoid it with CCW direction
		else

			% make smooth transition near the obstacle
			% boundary
			tmp_x = ( (dist-ro)*cos(angle-0.5*PI) + (ro+m-dist)*cos(angle-PI) ) / m;
			tmp_y = ( (dist-ro)*sin(angle-0.5*PI) + (ro+m-dist)*sin(angle-PI) ) / m;
			theta_d_new = atan2( tmp_y, tmp_x );
        end

	else

		% modify theta_d to avoid it with CW direction
		if diff_angle > 0.

			theta_d_new = fabs( atan( (ro+m) / sqrt(dist*dist - (ro+m)*(ro+m) ))) + angle;

		% modify theta_d to avoid it with CCW direction
		else

			theta_d_new = -fabs( atan( (ro+m) / sqrt(dist*dist - (ro+m)*(ro+m) ))) + angle;
        end
    end
    end
end
rul = Crul(0, 0, 0, (theta_d_new - theta_d) * coef1, 0);