ballbeam1.m

ball beam 球杆系统pid仿真程序

function ballbeam1(alpha,ballx,theta)
% BALLBEAM BALLBEAM(alpha,BALLX) displays the animated response
% of a ball and beam apparatus where alpha is the angle
 % of the beam in radians and BALLX is the distance of the
 % ball from the center of the beam in meters. The beam
 ballx=ballx*100; % convert from m to cm
 beam_length = 100; % cm
 bl2 = beam_length/2;
 radius = 4.0; % ball radius
 arcstep = 3; % angle increment (in degrees)
 % to sweep out arc lengths when
 % creating sides of ball
 lalpha = length(alpha); % find length of alpha
 bally =(100 - ballx).* tan(alpha) + radius; % find y positions of ball
 j = 0:arcstep:(360-arcstep); % create ball sides using arclengths
 arcx = radius * cos((j+arcstep) * pi/180); % x coordinates of ball sides
 arcy = radius * sin((j+arcstep) * pi/180); % y coordinates of ball sides
beamx1 = 0*alpha; % compute coordinates of beam
 beamx2 =100 * cos(alpha); % ends: left end => (beamx1,beamy1)
 beamy1 = 0*alpha; % right end => (beamx2,beamy2)
 beamy2 = 100 * sin(alpha);
 stickx1=beamx2;
 sticky1=beamy2;
 stickx2=beamx2;
 sticky2=beamy2-20;
 l1x=0*beamx2+95;
 l1y=0*beamx2-20;
 l2x=6.6*cos(theta)+95;
 l2y=6.6*sin(theta)-20;
 % construct initial beam using a wide line
 % LineWidth of 0.5 is normal
 L = plot([beamx1(1) beamx2(1)], [beamy1(1) beamy2(1)], 'y', 'EraseMode', 'xor','LineWidth',[2.5]);
 hold on;
 L1=plot([stickx1(1) stickx2(1)], [sticky1(1) sticky2(1)], 'y', 'EraseMode', 'xor','LineWidth',[2.5]);
 L2=plot([l1x(1) l2x(1)], [l1y(1) l2y(1)], 'b', 'EraseMode', 'xor','LineWidth',[2.5]);
 axis([-20 120 -50 50]); % construct square axes
 patch([-2 0 2 -2], [-5 0 -5 -5], 'y'); % construct beam stand
 % construct initial ball
 H = patch(arcx+ballx(1), arcy+bally(1), 'b', 'EraseMode', 'xor');
sita=0:pi/20:2*pi; 
plot(6.5*cos(sita)+95,6.5*sin(sita)-20,'LineWidth',[2.5]); 
for i = 2:lalpha, % loop over data values
    for j=0:4000000
    end
 set(L, 'XData', [beamx1(i) beamx2(i)]); % change x coordinates of beam ends
 set(L, 'YData', [beamy1(i) beamy2(i)]); % change y coordinates of beam ends
 set(L1, 'XData', [stickx1(i) stickx2(i)]); % change x coordinates of stick ends
 set(L1, 'YData', [sticky1(i) sticky2(i)]); % change y coordinates of stick ends
 set(L2, 'XData', [l1x(i) l2x(i)]); % change x coordinates of stick ends
 set(L2, 'YData', [l1y(i) l2y(i)]); % change y coordinates of stick ends
  set(H, 'XData', arcx+100-ballx(i)); % change x coordinate of ball
 set(H, 'YData', arcy+bally(i)); % change y coordinate of ball
 drawnow;
 end;
 hold off;