19.07.2013 Views

Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...

Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...

Kinematic and Dynamic Analysis of Spatial Six Degree of Freedom ...

SHOW MORE
SHOW LESS

You also want an ePaper? Increase the reach of your titles

YUMPU automatically turns print PDFs into web optimized ePapers that Google loves.

pty:=roy+dy;<br />

ptz:=roz+dz;<br />

end;<br />

function MyPoint.AbsDifference : double;<br />

begin<br />

AbsDifference := sqrt(sqr(tx-x)+sqr(ty-y)+sqr(tz-z));<br />

end;<br />

function MyPoint.Alpha : double;<br />

begin<br />

Alpha:=180/pi*ArcTan2(ptz,(ptx*py/r1-pty*px/r1));<br />

end;<br />

procedure InitConstParam;<br />

var<br />

code:integer;<br />

a1,a2,a3:double;<br />

begin<br />

a1:=90*pi/180;<br />

a2:=(90+120)*pi/180;<br />

a3:=(90+240)*pi/180;<br />

Val(InvMainForm.EditR1.Text,r1,code);<br />

if code 0 then ShowMessage('Illegal entry for R1');<br />

Val(InvMainForm.EditR2.Text,r2,code);<br />

if code 0 then ShowMessage('Illegal entry for R2');<br />

BasePoint[1].x:=r1*cos(a1);<br />

BasePoint[2].x:=r2*cos(a1);<br />

BasePoint[3].x:=r1*cos(a2);<br />

BasePoint[4].x:=r2*cos(a2);<br />

BasePoint[5].x:=r1*cos(a3);<br />

BasePoint[6].x:=r2*cos(a3);<br />

BasePoint[1].y:=r1*sin(a1);<br />

BasePoint[2].y:=r2*sin(a1);<br />

BasePoint[3].y:=r1*sin(a2);<br />

BasePoint[4].y:=r2*sin(a2);<br />

BasePoint[5].y:=r1*sin(a3);<br />

BasePoint[6].y:=r2*sin(a3);<br />

for code:=1 to 6 do BasePoint[code].z:=0;<br />

end;<br />

Procedure CalculateInputParameters;<br />

begin<br />

InvMainForm.EditP1.Text:=Format('%8.3f',[BasePoint[2].AbsDifference]);<br />

InvMainForm.EditP2.Text:=Format('%8.3f',[BasePoint[4].AbsDifference]);<br />

InvMainForm.EditP3.Text:=Format('%8.3f',[BasePoint[6].AbsDifference]);<br />

InvMainForm.EditM1.Text:=Format('%4.2f',[BasePoint[1].Alpha]);<br />

InvMainForm.EditM2.Text:=Format('%4.2f',[BasePoint[3].Alpha]);<br />

InvMainForm.EditM3.Text:=Format('%4.2f',[BasePoint[5].Alpha]);<br />

end;<br />

Procedure DoInverseTask;<br />

var<br />

i:integer;<br />

scale: real;<br />

begin<br />

InitConstParam;<br />

ReadTransformationParameters;<br />

For i:=1 to 6 do<br />

BasePoint[i].Transform(roll,pitch,yaw,rx,ry,rz);<br />

CalculateInputParameters;<br />

75

Hooray! Your file is uploaded and ready to be published.

Saved successfully!

Ooh no, something went wrong!