KiraYamato Posted August 18, 2011 Share Posted August 18, 2011 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 More sharing options...
Sina.Sattari Posted August 18, 2011 Share Posted August 18, 2011 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 More sharing options...
KiraYamato Posted August 20, 2011 Author Share Posted August 20, 2011 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 More sharing options...
Recommended Posts