JO1_Control/robocar_animation.asv at main · CEJ2-Robotics/JO1_Control · 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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
function robocar_animation(block)
    setup(block);
end

function setup(block)
    % number of ports
    block.NumInputPorts  = 4;  % Add a fourth port for waypoints
    block.NumOutputPorts = 0;

    block.SetPreCompInpPortInfoToDynamic;

    % parameters
    block.NumDialogPrms = 0;

    % sample times
    block.SampleTimes = [0.1 0];

    block.SimStateCompliance = 'DefaultSimState';

    % methods
    block.RegBlockMethod('Start', @start);
    block.RegBlockMethod('Outputs', @outputs);
    block.RegBlockMethod('Terminate', @terminate);
    block.RegBlockMethod('SetInputPortDimensions', @dims);
end

function start(~)
    persistent fig_handle;
end

function outputs(block)
    persistent fig_handle;

    state = block.InputPort(1).Data;
    L = block.InputPort(2).Data;
    waypoints = block.InputPort(3).Data;

    target = wapypoints(end-1)

    % check if figure still exists
    if isempty(fig_handle) || ~ishandle(fig_handle)
        fig_handle = figure('Name', 'Tricycle Animation');
        axis([-2 17 -2 32]);
        grid on;
        hold on;
        title('Tricycle Motion');
        xlabel('X Position [m]');
        ylabel('Y Position [m]');
    end

    figure(fig_handle);
    clf;
    hold on;
    grid on;

    % Plot waypoints with lines and points
    plot(waypoints(:,1), waypoints(:,2), 'g-o', 'MarkerSize', 4, 'DisplayName', 'Waypoints', 'LineWidth', 1);

    % call visualization function
    visualize_car(state, target, L);

    % update plot
    axis([-2 17 -2 32]);
    grid on;
    drawnow;
end

function terminate(~)
    % Clean up figure if needed
    persistent fig_handle;
    if ~isempty(fig_handle) && ishandle(fig_handle)
        close(fig_handle);
    end
    clear fig_handle;
end

function dims(block, idx, di)
    block.InputPort(idx).Dimensions = di;
end

function visualize_car(state, target, L)

    x = state(1);
    y = state(2);
    theta = state(3);

    % Tricycle visualization parameters
    wheel_width = L/5;
    body_width = L/2;

    % Create transformation matrix
    R = [cos(theta) -sin(theta); sin(theta) cos(theta)];

    % Define vehicle body points
    body = [-L/2 -body_width/2;
            L/2  -body_width/2;
            L/2   body_width/2;
            -L/2  body_width/2]';

    % Define front wheel
    front_wheel = [-wheel_width/2 -wheel_width/4;
                   wheel_width/2  -wheel_width/4;
                   wheel_width/2   wheel_width/4;
                   -wheel_width/2  wheel_width/4]';

    % Define rear wheels
    rear_wheel_left = front_wheel;
    rear_wheel_right = front_wheel;

    % Transform body
    body_trans = R * body;
    body_trans(1,:) = body_trans(1,:) + x;
    body_trans(2,:) = body_trans(2,:) + y;

    % Transform front wheel (with steering)
    front_wheel = R * front_wheel;
    front_wheel(1,:) = front_wheel(1,:) + x + L/2*cos(theta);
    front_wheel(2,:) = front_wheel(2,:) + y + L/2*sin(theta);

    % Transform rear wheels
    rear_wheel_left = R * rear_wheel_left;
    rear_wheel_left(1,:) = rear_wheel_left(1,:) + x - L/2*cos(theta) - body_width/2*sin(theta);
    rear_wheel_left(2,:) = rear_wheel_left(2,:) + y - L/2*sin(theta) + body_width/2*cos(theta);

    rear_wheel_right = R * rear_wheel_right;
    rear_wheel_right(1,:) = rear_wheel_right(1,:) + x - L/2*cos(theta) + body_width/2*sin(theta);
    rear_wheel_right(2,:) = rear_wheel_right(2,:) + y - L/2*sin(theta) - body_width/2*cos(theta);

    % Plot vehicle
    fill(body_trans(1,:), body_trans(2,:), 'b', 'FaceAlpha', 0.3)  % Vehicle body
    fill(front_wheel(1,:), front_wheel(2,:), 'k')  % Front wheel
    fill(rear_wheel_left(1,:), rear_wheel_left(2,:), 'k')  % Rear left wheel
    fill(rear_wheel_right(1,:), rear_wheel_right(2,:), 'k')  % Rear right wheel

    plot(target(1), target(2), 'r*', 'MarkerSize', 10)
end