matlab实现循迹小车程序源码.zip

function [sys, x0] = animtbu(t, x, u, flag, action) %ANIMTBU Animation of TBU (truck backer-upper) system. % This is animation of the truck backer-upper (TBU) system. By clicking % at the "Controller:" pop-up menu, you can choose to control this % system by yourself or by a fuzzy controller. If it is controlled by % human, click at the steering handle at the lower right corner to % change the steering angle of the truck's front wheels. % % The controller used here actually contains two state feedback % controllers (implemented as MATLAB function blocks); these two state % feedback controllers operate on two different sets of state variables. % The fuzzy controller is then used as a blender to combined these two % controllers smoothly based on the distance between the truck and the % loading duck. % % The simulation stops whenever the truck hits the back wall. Move the % truck by clicking inside the truck, or rotate it by clicking at one % of its corners. % % Animation S-function: animtbu.m % SIMULINK file: sltbu.m % Roger Jang, 10-21-93, 1-16-94, 11-13-94 % Copyright 1994-2001 The MathWorks, Inc. % $Revision: 1.12 $ $Date: 2001/04/06 14:08:20 $ % User data convention: % userdata = get(AnimTbuFigH, 'userdata'); % userdata(1, :) --> handles for standard SL gui control % userdata(2, :) --> handles for additional gui control % userdata(3, :) --> handles for animation objects global TbuInitCond TbuSlStatus TbuSteerAngle x0 global TbuTruck TbuCenter truck_l truck_w wheel_l wheel_w global TbuOriDispMode TbuCurrPt date num axisx axisy global AnimTbuFigH AnimTbuFigTitle AnimTbuAxisH TbuSteerCenter global AnimTbuTranslate AnimTbuRotate out sen ndate Phi0 global start Display RadaDetectData RadaRange mapH map j=sqrt(-1); %===============计算输出======== if ~isempty(flag) & flag == 3, sys = TbuSteerAngle; elseif ~isempty(flag) & flag == 2,%更新小车状态 if any(get(0, 'children') == AnimTbuFigH), if strcmp(get(AnimTbuFigH, 'Name'), AnimTbuFigTitle), xPos = u(1); yPos = u(2); phi = u(3); Display(2)=u(1);Display(3)= u(2);Display(4)= u(3); if length(u)==4, theta = u(4); else theta = 0; end curr_pos = xPos + j*yPos;phi0=phi+pi/2; tmp = get(AnimTbuFigH, 'UserData'); objectH = tmp(3, :); %============= 更新小车当前坐标===========% TbuCenterH = objectH(6); %质心句柄 TbuCenter = curr_pos; set(TbuCenterH, 'XData', real(TbuCenter), 'YData', imag(TbuCenter)); % ====== circle position truckH = objectH(1); %车体句柄 %truck_l = 2*abs(real(TbuTruck(1))); %truck_w = 2*abs(imag(TbuTruck(1))); new_truck = (TbuTruck)*exp(j*phi) + curr_pos; set(truckH, 'XData', real(new_truck), 'YData', imag(new_truck)); % ====== middle axis 中轴========= %wheel_axishH = objectH(6);%中轴句柄+ truck_l/2 %wheel_axish = get(wheel_axishH,'userdata'); %wheel_axish = wheel_axish*exp(j*phi0) + curr_pos; %车体转过角度phi后，新的车轴对应坐标，可以通过绘制wheel_axish以及rotated_wheel_axish来看出转角变化带来的影响。 %set(wheel_axishH,'XData',real(wheel_axish),'YData',imag(wheel_axish)); % ====== wheels 车轮=============== wheelH = objectH(2); wheel = get(wheelH, 'userdata'); %wheel_l = 2*abs(real(wheel)); %wheel_w = 2*abs(imag(wheel)); f_wheelsH = objectH(3); displace = (truck_l - wheel_l)/2 + j*truck_w/2; seperator = nan + j*nan; rotated_wheel = wheel*exp(j*(-theta)); wheel1 = -conj(displace) + rotated_wheel; wheel2 = -displace + rotated_wheel; f_wheels = [wheel1; seperator; wheel2]; % rotated front wheels new_f_wheels = (f_wheels+ truck_l/2)*exp(j*phi) + curr_pos; set(f_wheelsH, 'XData', real(new_f_wheels), 'YData',imag(new_f_wheels)); % ====== 车轮转角 r_wheelsH = objectH(4); wheel3 = displace + wheel; wheel4 = conj(displace) + wheel; r_wheels = [wheel3; seperator; wheel4]; % rear wheels new_r_wheels = (r_wheels+ truck_l/2)*exp(j*phi) + curr_pos; set(r_wheelsH, 'XData', real(new_r_wheels), 'YData',imag(new_r_wheels)); % ===== 雷达 RadaDetectH = objectH(5); %雷达句柄 % RadaDetect = get(RadaDetectH,'userdata'); RadaDetectRange = RadaRange * [exp(j*linspace(pi,0,37))]*exp(j*phi0)+curr_pos; % aa = linspace(pi,0,37); % bb=[aa(1),aa(9),aa(9:2:28),aa(28),aa(37)]; % RadaDetectRange = RadaRange * [exp(j*bb)]*exp(j*phi0)+TbuCenter; tmp = get(AnimTbuFigH, 'userdata'); showsensorH = tmp(2,2); showsensor= get(showsensorH,'value'); %if(showsensor ==0), % set(RadaDetectH, 'visible', 'off'); % else set(RadaDetectH, 'XData', real(RadaDetectRange), 'YData',imag(RadaDetectRange)); % set(RadaDetectH, 'XData', real(RadaDetectRange), 'YData',imag(RadaDetectRange), 'visible', 'on'); %end % set(RadaDetectH, 'XData', real(RadaDetectRange), 'YData',imag(RadaDetectRange)); % 改到这里了。viobstacles(n,:)=vicinage(ob(n,:),s01,s04,s08,s11,s14,s18,s21,s24,s28); %*********************************** % get(mobileobsH,'userdata') % obsH = objectH(7);%障碍句柄 %RadaDetectData = vicinage(Obstacle,RadaDetectRange,TbuCenter); sen=RadaDetectRange; %mapdate=get(mapH,'userdata') %ma=mapdate in=inpolygon(real(sen),imag(sen),real(map),imag(map)); date=sum(in); ndate=cumsum(in); for k=1:37 if(ndate(k)==1) num=k; else k=k+1; end end %n=1; %for k=1:37 % if n==1 % if(in(k)==1) % num=k;n=0 %end %end %end % inpolygon % % ====== update ball % truckH = objectH(1); % %TbuTruck = get(truckH, 'userdata'); % Pioneer_r = 2*abs(real(TbuTruck(1))); % truck_w = 2*abs(imag(TbuTruck(1))); % new_truck = (TbuTruck + Pioneer_r/2)*exp(j*phi) + curr_pos; % set(truckH, 'XData', real(new_truck), 'YData', imag(new_truck)); % % ====== front wheels % wheelH = objectH(4); % wheel = get(wheelH, 'userdata'); % wheel_l = 2*abs(real(wheel(1))); % wheel_w = 2*abs(imag(wheel(1))); % f_wheelsH = objectH(2); % displace = (Pioneer_r - wheel_l)/2 + j*truck_w/2; % seperator = nan + j*nan; % rotated_wheel = wheel*exp(j*theta); % wheel1 = displace + rotated_wheel; % wheel2 = conj(displace) + rotated_wheel; % f_wheels = [wheel1; seperator; wheel2]; % rotated front wheels % new_f_wheels = (f_wheels + Pioneer_r/2)*exp(j*phi) + curr_pos; % set(f_wheelsH, 'XData', real(new_f_wheels), 'YData',imag(new_f_wheels)); % % ====== rear wheels % r_wheelsH = objectH(3); % wheel3 = -conj(displace) + wheel; % wheel4 = -displace + wheel; % r_wheels = [wheel3; seperator; wheel4]; % rear wheels % new_r_wheels = (r_wheels + Pioneer_r/2)*exp(j*phi) + curr_pos; % set(r_wheelsH, 'XData', real(new_r_wheels), 'YData',imag(new_r_wheels)); % % ====== update handle % stickH = objectH(5); % stick = get(stickH, 'userdata'); % who_drive = get_param('sltbu/Constant', 'value'); % who_drive =1; % if str2double(who_drive) > 0, % not human controller % new_stick = stick*e