Simulated Mobile Robot Control (4) (Modified again)

Environment: win7;

Added Count display for analog Photoelectric Encoder

CLC; close all; clearsubplot (121); Title ('simulated robot coordinate '); hold onaxis ([0,130, 0,200]); t = 0.01; t = 1; [x, y] = ginput (1); C = PI/2; P = [x, y, C]; L = 4; V = 4; VL = V; Vr = V; PL = [X-L/2, y]; Pr = [x + L/2, y]; plot (PL (1) + Pr (1)/2, (PL (2) + Pr (2)/2, 'K * '); plot (PL (1), PL (2), 'ro '); plot (Pr (1), Pr (2), 'bo'); text (40,180, {strcat ('P = ', num2str (PL (1) + Pr (1)/2), ',', num2str (PL (2) + Pr (2)/2)}); Report (:, T) = [VL; VR; P (1); P (2)]; t = t + 1; subplot (122); % plot (1: T-1, report (1, :) * 10, 'R. ', 1: T-1, Report (2, :) * 10,' B. ', 1: T-1, report (3, :), 'G', 1: T-1, report (4, :), 'C '); title ('analog photoelectric encoder pulse count'); % legend ('revolver count', 'right wheel count', 'x', 'y', 'location ', 'northwest'); pause (t); While (1) subplot (121); [XT, yt] = ginput (1); % capture data with the mouse in figure, 1 indicates capturing Pt = [XT; yt]; % Pt = [XT; yt; CT] target point plot (Pt (1), Pt (2), 'K. ', 'linewidth', 4); D = (P (1)-Pt (1 )). ^ 2 + (P (2)-Pt (2 )). ^ 2 ). ^ 0.5; % (Pr (1)-PL (1 )). ^ 2 + (Pr (2)-PL (2 )). ^ 2 ). ^ 0.5 while (D> V) % keep moving at a speed greater than the target point if (Pt (1)> = P (1) & Pt (2)> = P (2) if (Pt (1)> = P (1) & Pt (2)> = P (2) Ct = atan (Pt (2) -P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2)> = P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2) <= P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1)> = P (1) & Pt (2) <= P (2 )) ct = atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % end elseif (Pt (1) <= P (1) & Pt (2)> = P (2 )) if (Pt (1)> = P (1) & Pt (2)> = P (2) Ct = atan (Pt (2)-P (2 )) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2)> = P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2) <= P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1)> = P (1) & Pt (2) <= P (2 )) ct = atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % end elseif (Pt (1) <= P (1) & Pt (2) <= P (2 )) if (Pt (1)> = P (1) & Pt (2)> = P (2) Ct = atan (Pt (2)-P (2 )) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2)> = P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2) <= P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1)> = P (1) & Pt (2) <= P (2 )) ct = 2 * PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % end elseif (Pt (1)> = P (1) & Pt (2) <= P (2 )) if (Pt (1)> = P (1) & Pt (2)> = P (2) Ct = atan (Pt (2)-P (2 )) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2)> = P (2 )) ct = PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1) <= P (1) & Pt (2) <= P (2 )) ct =-PI + atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % current and target point angle elseif (Pt (1)> = P (1) & Pt (2) <= P (2 )) ct = atan (Pt (2)-P (2) * (Pt (1)-P (1 )). ^ (-1); % end v = 0.2; W = 2 * V/L; while (C> (CT + 0.5 * W) | C <= (Ct-0.5 * w) % angle adjustment if (C> CT) n =-1; else n = 1; end VL =-1 * n * V; vr = N * V; DSL = L * sin (0.5 * ABS (w); DSR = L * sin (0.5 * ABS (w )); if (Pt (1)> = P (1) & Pt (2)> = P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); End elseif (Pt (1) <= P (1) & Pt (2)> = P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); End elseif (Pt (1) <= P (1) & Pt (2) <= P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); End elseif (Pt (1)> = P (1) & Pt (2) <= P (2) if (Pr (1)> = pl (1) & pr (2) >=pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); end C = C + PI/2; PL = pl + [-1 * n * DSL * Cos (C + N * w/2 ), -1 * n * DSL * sin (C + N * w/2)]; Pr = Pr + [N * DSR * Cos (C + N * w/2 ), N * DSR * sin (C + N * w/2)]; % (Pr (1)-PL (1 )). ^ 2 + (Pr (2)-PL (2 )). ^ 2 ). ^ 0.5 CLF; subplot (121); Title ('simulated robot coordinate '); Hold on axis ([0,130, 0,200]); text (20,180, {strcat ('destination seat: PT = ', num2str (Pt (1),', ', num2str (Pt (2),', ', 'ct =', num2str (CT/PI ), '* PI') strcat ('Robot seat: P = ', num2str (PL (1) + Pr (1)/2 ),',', num2str (PL (2) + Pr (2)/2), ',', 'c = ', num2str (C/PI),' * PI '), strcat ('left and right wheel speed: VL = ', num2str (VL),', ', 'vr =', num2str (VR), ',', 'W = ', num2str (w),}); plot (PL (1) + Pr (1)/2, (PL (2) + Pr (2)/2, 'K * '); plot (PL (1), PL (2), 'ro'); plot (Pr (1), Pr (2), 'bo '); plot (Pt (1), Pt (2), 'K. ', 'linewidth', 4); plot (Report (3, :), report (4, :)); P = [(PL (1) + Pr (1 )) /2, (PL (2) + Pr (2)/2, c]; D = (P (1)-Pt (1 )). ^ 2 + (P (2)-Pt (2 )). ^ 2 ). ^ 0.5; Report (:, t) = [VL; VR; P (1); P (2)]; t = t + 1; % subplot (122 ); % plot (1: T-1, report (1, :) * 10, 'R. ', 1: T-1, Report (2, :) * 10,' B. ', 1: T-1, report (3, :), 'G', 1: T-1, report (4, :), 'C '); title ('analog photoelectric encoder pulse count'); % legend ('revolver count', 'right wheel count', 'x', 'y', 'location ', 'northwest'); pause (t); End v = 4; VL = V + (2 * rand-1) * 0.05 * V; vr = V + (2 * rand-1) * 0.05 * V; W = (VR-VL)/L; RL = ABS (VL/W ); RR = ABS (VR/W); DSL = 2 * rl * sin (0.5 * ABS (w )); DSR = 2 * RR * sin (0.5 * ABS (w); If (Pt (1)> = P (1) & Pt (2)> = P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2) -PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); End elseif (Pt (1) <= P (1) & Pt (2)> = P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); End elseif (Pt (1) <= P (1) & Pt (2) <= P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C = PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); End elseif (Pt (1)> = P (1) & Pt (2) <= P (2) if (Pr (1)> = pl (1) & pr (2)> = pl (2) c = atan (Pr (2)-PL (2) * (Pr (1) -PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2)> = pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1) <= pl (1) & pr (2) <= pl (2 )) C =-PI + atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); elseif (Pr (1)> = pl (1) & pr (2) <= pl (2 )) C = atan (Pr (2)-PL (2) * (Pr (1)-PL (1 )). ^ (-1); end C = C + PI/2; PL = pl + [DSL * Cos (C + W/2 ), DSL * sin (C + W/2)]; Pr = Pr + [DSR * Cos (C + W/2), DSR * sin (C + W/2)]; CLF; subplot (121); Title ('simulated robot coordinates '); Hold on axis ([0,130, 0,200]); text (20,180, {strcat ('destination seat: PT = ', num2str (Pt (1),', ', num2str (Pt (2),', ', 'ct =', num2str (CT/PI ), '* PI') strcat ('Robot seat: P = ', num2str (PL (1) + Pr (1)/2 ),',', num2str (PL (2) + Pr (2)/2), ',', 'c = ', num2str (C/PI),' * PI '), strcat ('left and right wheel speed: VL = ', num2str (VL),', ', 'vr =', num2str (VR), ',', 'W = ', num2str (w),}); plot (PL (1) + Pr (1)/2, (PL (2) + Pr (2)/2, 'K * '); plot (PL (1), PL (2), 'ro'); plot (Pr (1), Pr (2), 'bo '); plot (Pt (1), Pt (2), 'K. ', 'linewidth', 4); plot (Report (3, :), report (4, :)); P = [(PL (1) + Pr (1 )) /2, (PL (2) + Pr (2)/2, c]; D = (P (1)-Pt (1 )). ^ 2 + (P (2)-Pt (2 )). ^ 2 ). ^ 0.5; Report (:, t) = [VL; VR; P (1); P (2)]; t = t + 1; % subplot (122 ); % plot (1: T-1, report (1, :) * 10, 'R. ', 1: T-1, Report (2, :) * 10,' B. ', 1: T-1, report (3, :), 'G', 1: T-1, report (4, :), 'C '); title ('analog photoelectric encoder pulse count'); % legend ('revolver count', 'right wheel count', 'x', 'y', 'location ', 'northwest'); pause (t); End subplot (122); plot (1: T-1, report (1, :) * 10, 'R. ', 1: T-1, Report (2, :) * 10,' B. ', 1: T-1, report (3, :), 'G', 1: T-1, report (4, :), 'C '); title ('analog photoelectric encoder pulse count'); legend ('revolver count', 'right wheel count', 'x', 'y', 'location', 'northwest '); end

Result: