Jump to content
OMRON Forums

How to make a Kinematic Program solution?


KiraYamato

Recommended Posts

Hello, everyone

 

I am trying to use Turbo PMAC 2 Clipper to control the servo motors in a robot system,

 

I find the the example in Turbo PMAC USER MANUAL from page 261 to 266,

 

and I get the codes.

 

Now my question comes:

 

How to deal with these code?

 

Should I piece them together as a *.pmc file and download it directly?

 

Or should I down load it separately,just like the *.h file and *.cpp?

 

And where should I write my codes for motion program ?

 

I attach the codes in the end

 

Thank you for your attention~

 

;---------------------------------------------------[/size][/font]

 

; Setup for program

I15=0 ; Trig calculations in degrees

&1 ; Address CS 1

M145->Y:$0000C0,10,1 ; Motor 1 home complete bit

M245->Y:$000140,10,1 ; Motor 2 home complete bit

M5182->Y:$00203F,22,1 ; CS 1 run-time error bit

Q91=400 ; L1

Q92=300 ; L2

Q93=1000 ; Counts per degree for A and B

; Forward-kinematic program buffer for repeated execution

&1 OPEN FORWARD ; Forward kinematics for CS 1

CLEAR ; Erase existing contents

IF (M145=1 AND M245=1) ; Properly position referenced?

Q7=Q91*COS(P1/Q93)+Q92*COS((P1+P2)/Q93) ; X position

Q8=Q91*SIN(P1/Q93)+Q92*SIN((P1+P2)/Q93) ; Y position

ELSE ; Not valid; halt operation

M5182=1 ; Set run-time error bit

ENDIF

CLOSE

 

; M-variable definitions for actual position registers

M162->D:$8B ; Motor 1 actual position

M262->D:$10B ; Motor 2 actual position

; Forward-kinematic PLC program buffer for position reporting

OPEN PLC 10 ; Forward kinematics for CS 1

CLEAR ; Erase existing contents

P51=M162/(I108*32*Q93) ; Actual A position (deg)

P52=M262/(I208*32*Q93) ; Actual B position (deg)

Q27=Q91*COS(P51)+Q92*COS(P51+P52) ; Actual X position

Q28=Q91*SIN(P51)+Q92*SIN(P51+P52) ; Actual Y position

CLOSE

 

; Setup for program

&1

#1->I ; Motor 1 assigned to inverse kinematic axis in CS 1

#2->I ; Motor 2 assigned to inverse kinematic axis in CS 1

M5182->Y:$00203F,22,1 ; CS 1 run-time error bit

; Pre-compute additional system constants

Q94=Q91*Q91+Q92*Q92 ; L1^2 + L2^2

Q95=2*Q91*Q92 ; 2*L1*L2

Q96=Q91*Q91-Q92*Q92 ; L1^2 – L2^2

; Inverse-kinematic algorithm to be executed repeatedly

&1 OPEN INVERSE ; Inverse kinematics for CS 1

CLEAR ; Erase existing contents

Q20=Q7*Q7+Q8*Q8 ; X^2+Y^2

Q21=(Q20-Q94)/Q95 ; cos(B)

IF (ABS(Q21)<0.9998) ; Valid solution w/ 1 deg margin?

Q22=ACOS(Q21) ; B (deg)

Q0=Q7 ; X into cos argument for ATAN2

Q23=ATAN2(Q8) ; A+C = ATAN2(Y,X)

Q24=ACOS((Q20+Q96)/(2*Q91*SQRT(Q20))) ; C (deg)

Q25=Q23-Q24 ; A (deg)

P1=Q25*Q93 ; Motor 1 = 1000A

P2=Q22*Q93 ; Motor 2 = 1000B

ELSE ; Not valid, halt operation

M5182=1 ; Set run-time error bit

ENDIF

CLOSE

 

&1

OPEN INVERSE

CLEAR

{Position calculations from above}

IF (Q10=1) ; PVT mode?

Q26=SIN(Q25) ; sin(B)

IF (Q26>0.0175) ; Not near singularity?

Q27=Q91*Q92*Q26 ; L1*L2*sinB

Q28=COS(Q25+Q22) ; cos(A+B)

Q29=SIN(Q25+Q22) ; sin(A+B)

Q30=(Q92*Q28*Q17+Q92*Q29*Q18)/Q27 ; dA/dt

Q31=(-Q7*Q17-Q8*Q18)/Q27 ; dB/dt

P101=Q30*Q93 ; #1 speed in cts/(Isx90 msec)

P102=Q31*Q93 ; #2 speed in cts/(Isx90 msec)

ELSE ; Near singularity

M5182=1 ; Set run-time error bit

ENDIF

ENDIF

CLOSE

 

 

Link to comment
Share on other sites

  • Replies 2
  • Created
  • Last Reply

Top Posters In This Topic

Top Posters In This Topic

KiraYamato,

 

The forward and inverse kinematics can be downloaded as one file or separate files, as long as both routines are downloaded and stored in PMAC before executing a motion program. There can only be one inverse and one forward routine available for each coordinate system. In the example code that you have posted, there are 2 inverse routines, where the second one will replace the first one because of using CLEAR command when buffer was opened.

 

To learn more about motion programs, please read the chapter called "WRITING AND EXECUTING MOTION PROGRAMS" in Turbo PMAC Users Manual.

 

Link to comment
Share on other sites

KiraYamato,

 

The forward and inverse kinematics can be downloaded as one file or separate files, as long as both routines are downloaded and stored in PMAC before executing a motion program. There can only be one inverse and one forward routine available for each coordinate system. In the example code that you have posted, there are 2 inverse routines, where the second one will replace the first one because of using CLEAR command when buffer was opened.

 

To learn more about motion programs, please read the chapter called "WRITING AND EXECUTING MOTION PROGRAMS" in Turbo PMAC Users Manual.

 

 

Sina:

 

Thank you for your edification.

I found an easy example about kinematic program, and it works.

And I followed your instruction and I can run the program given in the user manual.

Now I will try to modify the code so that it will our virtual equipment, and my friend is dealing with the downloading of the program.

Thanks again~

Link to comment
Share on other sites

Guest
This topic is now closed to further replies.

×
×
  • Create New...