六杆机构MATLAB编程

function Six_main()

clear;

%输入数据

hd=pi/180;%--------弧度

du=180/pi;%--------度

EX=0;%-------------点E的X坐标值

EY=0;%-------------点E的Y坐标值

FX=0;%-------------点F的X坐标值

FY=-34;%-----------点F的Y坐标值

BX=41;%------------点B的X坐标值

BY=0;%-------------点B的Y坐标值

Led=14;%-----------杆ED的长度

Lda=39;%-----------杆AD的长度

Lba=28;%-----------杆AB的长度

Jadc=35;%----------角adc的大小

Ldc=15;%------------杆DC的长度

Lfg=55;%------------杆FG的长度

w1=10;%-------------曲柄1角速度

%调用子函数画图

for n1=1:720

fai1(n1)=(n1-1)*hd;

[ fai4(n1) DX(n1) DY(n1) CX(n1) CY(n1) fai3(n1) fai2(n1) Sc(n1) w2(n1) w3(n1) VH(n1) w4(n1) AW4(n1) AH(n1) ]=jisuan ( fai1(n1), Led, Lda, BX, BY, EX, EY, FX, FY,Lba,Ldc,Jadc,hd ,w1);

%w2、w3、w4杆2、3、4的角速度、VH滑块线速度、AW4杆4角加速度、AH滑块线加速度

end

h = figure(1); %--------------------窗口1

set( h, 'Color', [ 1 1 1 ] );

set (gcf,'Position',[500,300,950,600]);

aviobj = avifile('test3.avi','compression','None');

m = moviein(72);

f=0;

for n2=1:5:720

f=f+1;

subplot(2,2,1); %---------------fai3、fai4角位移、滑块线位移

[AX,H1,H2]=plotyy(fai1*du,[fai3*du;fai4*du;fai2*du],fai1*du,Sc);

set(H1,'color','r');

set(H2,'color','b');

box off;

grid on;

set(gca,'XGrid','off','YGrid','on');

set(AX(1),'YLim',[0 500]);%----------Y轴的数据显示范围

set(AX(1),'YTick',[0:100:500]);%-----设置要显示坐标刻度

set(AX(2),'YLim',[25 50]);%----------Y轴的数据显示范围

set(AX(2),'YTick',[25:5:50]);%-----设置要显示坐标刻度

hold on;

xlabel('曲柄转角\phi_1/\circ');

ylabel('角\phi_2、\phi_3、\phi_4单位/\circ');

title('角位移线图');

set(get(AX(2),'ylabel'),'string','滑块线速度/mm/s')

set(get(AX(2),'ylabel'),'color','b');

set(AX(2),'ycolor','b');

plot(fai1(n2)*du,fai3(n2)*du,'b*');

plot(fai1(n2)*du,fai4(n2)*du,'bs');

plot(fai1(n2)*du,fai2(n2)*du,'bo');

text(650,480,'* --φ3','fontsize',10,'fontname','楷体','color','k');

text(650,450,'口--φ4','fontsize',10,'fontname','楷体','color','k');

text(650,420,'o --φ2','fontsize',10,'fontname','楷体','color','k');

hold off;

%机构位姿

subplot(2,2,2);%------------------------------机构位姿

%坐标点计算

x(1)=0;

y(1)=0;

x(2)=Led*cos(fai1(n2));

y(2)=Led*sin(fai1(n2));

x(3)=DX(n2)+Lda*cos(fai3(n2));

y(3)=DY(n2)+Lda*sin(fai3(n2));

x(4)=DX(n2)+Ldc*cos(fai3(n2)+Jadc*hd);

y(4)=DY(n2)+Ldc*sin(fai3(n2)+Jadc*hd);

x(5)=Led*cos(fai1(n2));

y(5)=Led*sin(fai1(n2));

x(7)=0; %---------------------------------摇杆

y(7)=-34;

x(8)=55*cos(fai4(n2))+0;

y(8)=55*sin(fai4(n2))-34;

x(9)=x(4)+4*cos(fai4(n2)-30*hd); %----------滑块

y(9)=y(4)+4*sin(fai4(n2)-30*hd);

x(10)=x(4)+4*cos(fai4(n2)+30*hd);

y(10)=y(4)+4*sin(fai4(n2)+30*hd);

x(11)=x(4)+4*cos(fai4(n2)+150*hd);

y(11)=y(4)+4*sin(fai4(n2)+150*hd);

x(12)=x(4)+4*cos(fai4(n2)+210*hd);

y(12)=y(4)+4*sin(fai4(n2)+210*hd);

x(13)=x(9);

y(13)=y(9);

x(14)=DX(n2)+Lda*cos(fai3(n2));

y(14)=DY(n2)+Lda*sin(fai3(n2));

x(15)=41;

y(15)=0;

%------------画图-------画图

k=1:2;

plot(x(k),y(k),'r');

grid on;

hold on;

k=2:5; % 画DAC刚体三角形

plot(x(k),y(k),'b');

k=14:15;

plot(x(k),y(k),'g');

k=7:8;

plot(x(k),y(k),'g');

k=9:13;

plot(x(k),y(k),'r');

plot(x(1),y(1),'o');

plot(x(2),y(2),'o');

plot(x(3),y(3),'o');

plot(x(4),y(4),'o');

plot(x(15),y(15),'o');

plot(x(7),y(7),'o');

plot(x(1),y(1),'*');

plot(x(15),y(15),'*');

plot(x(7),y(7),'*');

hold off;

box off;

set(gca,'XLim',[-20 50]);%X轴的数据显示范围

set(gca,'XTick',[-20:10:50]);%设置要显示坐标刻度

set(gca,'YLim',[-40 20]);%X轴的数据显示范围

set(gca,'YTick',[-40:10:20]);%设置要显示坐标刻度

title('机构简图仿真');

ylabel('单位／mm');

xlabel('单位／mm');

text(28,15,'中北大学','fontsize',18,'fontname','楷体','fontweight','bold','color','m');

subplot(2,2,3); %----------待修改---------------w3、w4角速度、滑块线速度

[AX,H1,H2,]=plotyy(fai1*du,[w2;w4],fai1*du,VH/1000);

grid on;

set(gca,'XGrid','off','YGrid','on');

set(H2,'color','b');

set(H1,'color','r');

box off;

hold on;

plot(fai1(n2)*du,w2(n2),'b*');

plot(fai1(n2)*du,w4(n2),'bs');

hold off;

text(650,9,'* --w2','fontsize',10,'fontname','楷体','color','k');

text(650,7,'口--w4','fontsize',10,'fontname','楷体','color','k');

set(AX(2),'YLim',[-0.3 0.3]);%----------X轴的数据显示范围

set(AX(2),'YTick',[-0.3:0.1:0.3]);%-----设置要显示坐标刻度

title('角速度及滑块线速度');

xlabel('曲柄转角\phi_1/\circ');

ylabel('角速度/rad/s')

set(get(AX(2),'ylabel'),'string','滑块线速度/m/s');

set(get(AX(2),'ylabel'),'color','b');

set(AX(2),'ycolor','b');

subplot(2,2,4); %----------待修改---------------摇杆4角加速度、滑块线加速度

[AX,H1,H2,]=plotyy(fai1*du,AW4,fai1*du,AH);

set(H1,'color','r');

set(H2,'color','b');

grid on;

set(gca,'XGrid','off','YGrid','on');

hold on;

plot(fai1(n2)*du,AW4(n2),'b*');

hold off;

box off;

title('角加速度、线加速度图');

xlabel('曲柄转角\phi_1/\circ');

set(get(AX(2),'ylabel'),'color','b');

set(AX(2),'ycolor','b');

set(AX(1),'ycolor','k');

set(get(AX(2),'ylabel'),'string','滑块线加速度/m/s^');

set(get(AX(1),'ylabel'),'string','摇杆4角加速度/rad\cdots^{-2}');

set(AX(1),'YLim',[-80 60]);%----------Y轴的数据显示范围

set(AX(1),'YTick',[-80:20:60]);%-----设置要显示坐标刻度

set(AX(2),'YLim',[-1 2.5]);%----------Y轴的数据显示范围

set(AX(2),'YTick',[-1:0.5:2.5]);%-----设置要显示坐标刻度

text(590,50,'* --角加速度','fontsize',10,'fontname','楷体','color','k');

m(f)=getframe(gcf);

aviobj = addframe(aviobj,m(f));

end

aviobj = close(aviobj);

movie(m,100,16)

end

function [ fai4 DX DY CX CY fai3 fai2 Sc w2 w3 VH w4 AW4 AH ] = jisuan (fai1, Led, Lda, BX, BY, EX, EY, FX, FY,Lba, Ldc,Jadc,hd, w1)

DX = Led * cos(fai1);

DY = Led * sin(fai1);

Ldb=sqrt((DX-BX)*(DX-BX)+(DY-BY)*(DY-BY));

fai3 = 2*pi-( acos(( Lda*Lda+Ldb*Ldb-Lba*Lba )/(2*Lda*Ldb)) -atan((BY-DY)/(BX-DX)));

AX=DX+Lda*cos(fai3);

AY=DY+Lda*sin(fai3);

CX = DX+Ldc*cos(fai3+Jadc*hd);

CY = DY+Ldc*sin(fai3+Jadc*hd);

Sc = sqrt((CX-FX)*(CX-FX)+(CY-FY)*(CY-FY));

fai4=acos( (CX-FX)/sqrt((CX-FX)*(CX-FX)+(CY-FY)*(CY-FY)));

fai2 = acos((28*28+16- ((AX-45)*(AX-45)+(AY-BY)*(AY-BY)))/(2*28*4) );

else if AY

fai2 = 2*pi-acos((28*28+16- ((AX-45)*(AX-45)+(AY-BY)*(AY-BY)))/(2*28*4) );

else

fai2=pi;

end

end

%---------------计算速度、角速度

VD=Led*w1;

VDX=VD*cos(pi/2+fai1);

VDY=VD*sin(pi/2+fai1);

w2=(VDX*(AX-DX)+VDY*(AY-DY))/((AY-DY)*(AX-BX)-(AY-BY)*(AX-DX));

w3=(VDX*(AX-BX)+VDY*(AY-BY))/((AY-DY)*(AX-BX)-(AY-BY)*(AX-DX));

VCX=VDX-w3*(CY-DY);

VCY=VDY-w3*(CX-DX);

VH=(VCY*(CY+34)-VCX*CX)/(CX*cos(fai4)+(CY+34)*sin(fai4));

w4=(VCY*cos(fai4)-VCX*sin(fai4))/(CX*cos(fai4)+(CY+34)*sin(fai4));

%------------加速度、角加速度

AW3= (Lba*w2*w2-Led*w1*w1*cos(fai1-fai2)-Lda*w3*w3*cos(fai3-fai2))/(Lda*sin(fai3-fai2)) ; %--------连杆3角加速度

ACX = Led*w1*w1*cos(pi+fai1)-w3*w3*(CX-DX)-AW3*(CY-DY);%-----C加速度

ACY = Led*w1*w1*sin(pi+fai1)-w3*w3*(CY-DY)+AW3*(CX-DX);

E = ACX+w4*w4*(CX-0)+2*w4*VH*sin(fai4);

F = ACY+w4*w4*(CY+34)-2*w4*VH*cos(fai4);

AW4 = (-E*sin(fai4)+F*cos(fai4))/(CX*cos(fai4)+(CY+34)*sin(fai4));

AH = ((E*CX+F*(CY+34))/(CX*cos(fai4)+(CY+34)*sin(fai4)))/1000;

end