1 / 29

SPiiPlus Training Class

SPiiPlus Training Class. Advanced Motion Generation CONNECT Command. What is Motion Profile Generation?. Motion profile generation is the process by which the controller takes a high level command and creates a finely sampled motion profile

jiro
Download Presentation

SPiiPlus Training Class

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. SPiiPlus Training Class Advanced Motion Generation CONNECT Command

  2. What is Motion Profile Generation? Motion profile generation is the process by which the controller takes a high level command and creates a finely sampled motion profile • Profiles created using SPiiPlus motion generator are 3rd order with defined commanded position, velocity, acceleration, and jerk • Multi-axis motion is done using a vector parameterization • All axes start and stop at the same time • Results in linear motion in the virtual n-dimensional space • Profiles are for the ideal world – they do not compensate for mechanical or sensor errors • Profiles do not take into account robotic kinematics

  3. Motion Generation: SPiiPlus User Commands / External Signals Master Formula Master Position • (MPOS) MPU Motion Generator Axis Position (APOS) CONNECT formula Reference Position (RPOS) Feedback Position (FPOS) Convert counts to units (EFAC) Convert units to counts (EFAC) ECAT ECAT ECAT SP1 SP0 Drive Command Drive Command Feedback Feedback

  4. Motion Vector Parameterization • Multi-axis motion can involve any number of axes, but all axes start and stop at the same time • Motion parameters of the leading axis (first one listed) define the parameters for the vector move • As an example, assume axes 0, 1 and 2 start at position (0,0,0) and the following command is sent: PTP(0,1,2), 30, 40, 120 • The total move distance can be determined as the Euclidean distance • Assuming the commanded velocity of the leading axis (axis 0) is 100, what is the commanded velocity for each axis?

  5. Motion Vector Parameterization (30, 40, 120) (0, 0, 0)

  6. How to Command Non-Linear Actuator Motion? • In most applications, the normal linear vector motion is sufficient, however, there are times when non-linear motion is required • Flying Shear (cutting) • Feed-to-Length • Autofocus • Mechanical or sensor error mapping • Robot kinematics • SPiiPlus controllers include a unique command called CONNECT that simplifies the setup for these types of applications

  7. CONNECT Command • The CONNECT command defines the interface between the SPiiPlus motion generator output (APOS) and the servo processor commanded position (RPOS) • By default, the following CONNECT function is in use CONNECTRPOS(axis) = APOS(axis) • However, any formula can be used to update the servo processor command (RPOS) and it doesn’t have to include the output of the SPiiPlus motion generator (APOS) CONNECTRPOS(axis) = F(...) • The CONNECT command is executed once, and afterwards the firmware evaluates the CONNECT formula every controller cycle to update RPOS (as well as RVEL and RACC). • The active CONNECT formula can be queried by checking the axis status in the terminal ?$[axis number]

  8. CONNECT Command • It is sometimes necessary to switch between the default and non-default CONNECT formula, so a special bit flag is included in the variable MFLAGS • If MFLAGS().#DEFCON is set to 1, the default CONNECT formula is forced (RPOS = APOS) • On controller power-up MFLAGS().#DEFCON is forced to 1, regardless of what was saved in flash • Before a non-default CONNECT formula can be executed, MFLAGS().#DEFCON must be cleared • Whenever it is necessary to switch back to the default CONNECT formula, MFLAGS().#DEFCON must be set • Note: when switching back to the default CONNECT formula, APOS is automatically adjusted to equal RPOS

  9. CONNECTCommand – Example 1 • A simple example is shown on the right • An electronic gearing of 2:1 is forced • If the following command is sent, how far would the axis move? PTP/r (axis), 100 • If VEL(axis) is set to 100, what velocity will be sent to the servo processor (RVEL)? ! Variable Declaration globalint axis ! Variable initialization axis = 0 ! Toggle default CONNECT bit MFLAGS(axis).#DEFCON = 1 MFLAGS(axis).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(axis) = APOS(axis) * 2 STOP

  10. CONNECTCommand – Example 2 • A CONNECT formula is defined on the right that is the summation of two profiles: APOS(a1) + APOS(a2) • What would happen if the following command is sent? JOG/v (a1), 1000 • After the axis is at a constant velocity from the previous command, what would be the effect of the following command? PTP/r (a2), 20 ! Variable Declaration globalint a1, a2 ! Variable initialization a1 = 0; a2 = 1 ! Toggle default CONNECT bit MFLAGS(a1).#DEFCON = 1 MFLAGS(a1).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(a1) = APOS(a1) + APOS(a2) STOP

  11. CONNECT Offset CONNECTRPOS(axis) = F(...) • When executing a non-default CONNECT command, it is very likely the left and the right side of the formula are not equivalent. • For error-mapping, this difference might be small • For robot kinematics, the difference could be very large • How can the firmware handle this offset? • It could force the left side (RPOS) to equal the right (formula) • This will force a jump in the motor and could force the motor to move to a position not in the valid range of travel • It could force the right side (formula) equal to the left (RPOS) • Formula could be a function of many variables, be non-linear, etc, making it possible to have no solutions, one solution, or many solutions, and the firmware has no idea how to solve • It could add an offset term to force the two sides to be equal CONNECTRPOS(axis) = F(...) + ROFFS(axis) • This is how the SPiiPlus firmware works

  12. ROFFS Zeroing CONNECTRPOS(axis) = F(...) + ROFFS(axis) • In general, introducing an offset to the CONNECT formula is undesirable • The effect is the same as translating the origin by the ROFFS amount • Ideally ROFFS should be zero • How can the user application handle in SPiiPlus controllers? • Ideally, inverse formula is known so the right side (formula) can be solved based on the left side (RPOS), for any value of RPOS • Not always possible in the general case, but it will be known for a subset of values of RPOS • User application can first move the axis to a value that can be solved, and then execute the non-default CONNECT command • Numerical solvers could also be used to converge on a solution • The SPiiPlus firmware includes a numerical solver called MATCH that can be used to solve for formulas that are only a function of one variable, APOS

  13. ROFFSZeroing – Example 1 • A simple CONNECT formula is shown on the right • An electronic gearing of 2:1 is forced • If prior to executing the CONNECT command RPOS was set to 100, what is the value of APOS after it is executed? What is the value of ROFFS? • Is there an inverse formula so that APOS can be solved for any value of RPOS? ! Variable Declaration globalint axis ! Variable initialization axis = 0 ! Toggle default CONNECT bit MFLAGS(axis).#DEFCON = 1 MFLAGS(axis).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(axis) = APOS(axis) * 2 STOP

  14. ROFFSZeroing – Example 2 • A CONNECT formula is defined on the right that is the summation of two profiles: APOS(a1) + APOS(a2) • Can the values of APOS(a1) and APOS(a2) be solved uniquely for any value of RPOS(a1)? • This type of CONNECT formula is typically used for doing profile correction. If APOS(a2) can be set to zero when the CONNECT command is executed, can APOS(a1) be uniquely solved for? ! Variable Declaration globalint a1, a2 ! Variable initialization a1 = 0; a2 = 1 ! Toggle default CONNECT bit MFLAGS(a1).#DEFCON = 1 MFLAGS(a1).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(a1) = APOS(a1) + APOS(a2) STOP

  15. ROFFSZeroing – Example 3 • A CONNECT formula is defined on the right that uses a 1D error map to correct for repeatable encoder inaccuracy • Can the values of APOS(X) be solved uniquely for any value of RPOS(X)? • Are there values with no solution? • Are there values with multiple solutions? ! Variable Declaration globalint X globalrealErrorMapIn(11) globalrealErrorMapErr(11) ! Variable initialization X = 0 ! Toggle default CONNECT bit MFLAGS(X).#DEFCON = 1 MFLAGS(X).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(X) = APOS(X) + MAPN(APOS(X), ErrorMapIn, ErrorMapErr) STOP

  16. ROFFS Zeroing – Example 3 Cont’d 1. APOS(X) = RPOS(X) - MAPN(APOS(X), ErrorMapIn, ErrorMapErr) 2. APOS(X) = RPOS(X) - MAPN(RPOS(X), ErrorMapIn, ErrorMapErr) • A common misconception to solving for APOS(X) would be to use one of the following above. However, neither works. • Formula 1 does not work because APOS(X) is on both the left and right side of the formula, so the solution will by dependent on the initial ‘guess’ of APOS(X) • For example, assume to begin with that APOS(X) = 100, RPOS(X) = 100, and MAPN(100,...)= 5 • Then APOS(X) = RPOS(X) - MAPN(100,...) = 100 - 5 = 95 • But what if APOS(X) was initially 200, not 100? • Then APOS(X) = RPOS(X) - MAPN(200,...) = 100 - ?? • The initial ‘guess’ of APOS(X) cannot be guaranteed

  17. ROFFS Zeroing – Example 3 Cont’d 1. APOS(X) = RPOS(X) - MAPN(APOS(X), ErrorMapIn, ErrorMapErr) 2. APOS(X) = RPOS(X) - MAPN(RPOS(X), ErrorMapIn, ErrorMapErr) • Formula 2 does not work because RPOS(X) is not the input to the MAPN function, and the forward solution will not equal the inverse solution. • For example, assume to begin with that APOS(X) = 100, RPOS(X) = 100, and MAPN(100,...)= 5 • Then APOS(X) = RPOS(X) - MAPN(100,...) = 100 - 5 = 95 • Now check the forward solution • RPOS(X) = APOS(X) + MAPN(APOS(X),...) = 95 + ?? • Unless MAPN(100,...) = MAPN(95,...), the solution will not work • What to do?

  18. ROFFS Zeroing – Example 3 Cont’d • Assume the table above is used to define the error mapping in the code on the previous slide. • Can the values of APOS(X) be solved uniquely for any value of RPOS(X)? • What value of APOS(X) corresponds to RPOS(X) = -800?

  19. ROFFS Zeroing – Example 3 Cont’d • Assume the table above is used to define the error mapping in the code on the previous slide. • Can the values of APOS(X) be solved uniquely for any value of RPOS(X)?

  20. MATCH Command • In the previous example we couldn’t determine an analytical solution to solve for APOS(X) as a function of RPOS(X) • In that example, at least one solution always exists, but it is possible that 2 or more solutions also work, depending on the error mapping array • Because of the physical nature of error-mapping, there should always only be 1 solution • How to find that solution? • The SPiiPlus firmware includes a numerical solver command called MATCH that will converge on a solution for APOS(X) as a function RPOS(X) • Input to numerical solver is a range of values (from, to) to search for a solution • If multiple solutions exist, only 1 will be returned • Only works for CONNECT formulas that are a function of one variable, APOS(X) • Does not work for 2D error mapping

  21. MATCHCommand – Example 1 • A CONNECT formula is defined on the right that uses a 1D error map to correct for repeatable encoder inaccuracy • MATCH function solves for the correct value of APOS(X), regardless of the value of RPOS(X) • Why does the SETAPOS(X) come after the CONNECT command? ! Variable Declaration globalint X globalrealErrorMapIn(11) globalrealErrorMapErr(11) ! Variable initialization X = 0 ! Toggle default CONNECT bit MFLAGS(X).#DEFCON = 1 MFLAGS(X).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(X) = APOS(X) + MAPN(APOS(X), ErrorMapIn, ErrorMapErr) ! Determine value of APOS SETAPOS(X) = MATCH(X, -1000, 1000) STOP

  22. ROFFS Zeroing • What do you do when you do not have an analytical solution to solve the right side of the CONNECT formula, and the right side is a function of more than 1 variable (2D error mapping)? • Use more complex numerical solver. • Would have to be included in user code if done on the controller, or it could be done in the host application • Many different numerical solvers; outside of the scope of this presentation • Use a known nearby solution. • Guess the values of the variables on the right side of the CONNECT formula that result in a RPOS nearby • Move to the nearby RPOS • Execute CONNECT command and set the variables as the guessed value

  23. ROFFSZeroing – Example 4 ! Variable Declaration globalint X, Y globalrealEMapErr(5)(5) globalrealEMapErrInX(5) globalrealEMapErrInY(5) globalrealGuessX, GuessY ! Variable initialization X = 0; Y = 1 ! Toggle default CONNECT bit MFLAGS(X).#DEFCON = 1 MFLAGS(X).#DEFCON = 0 ! Guess APOS and move to corresponding RPOS GuessX = APOS(X); GuessY = APOS(Y) PTP/e (X), GuessX + MAP2N(GuessX, GuessY, EMapErr, EMapErrInX, EMapErrInY) ! Set non-default CONNECT formula CONNECTRPOS(X) = APOS(X) + MAP2N(APOS(X), APOS(Y), EMapErr, EMapErrInX, EMapErrInY) SETAPOS(X) = GuessX; STOP

  24. CONNECT Recalculation CONNECTRPOS(axis) = F(...) + ROFFS(axis) • After the CONNECT command is executed, the firmware will update RPOS every controller cycle according to the CONNECT formula above, except in the following circumstances: • Axis is disabled • RPOS will be set equal to FPOS • KILL command is executed on the axis • RPOS will be updated directly from motion generator (required for safety) • CONNECT command is executed on the axis • RPOS will not change • SET command is executed on the axis • If SETRPOS command is used, RPOS will be updated accordingly, or else it will not change • In any of those circumstances, ROFFS will be recalculated as ROFFS(axis) = RPOS(axis) - F(...)

  25. CONNECT Recalculation • Because it is usually desired to zero ROFFS, whenever a CONNECT recalculation event occurs, the application needs to re-zero ROFFS • Many safety events have a default response to KILL the axis. • Typically desired behavior – bypasses CONNECT formula to guarantee the axis comes to a complete stop • Sometimes this behavior is not needed (error mapping for example) so the default response should be turned off and a user routine should instead use the HALT command • Disabling the axes should be avoided when possible

  26. DEPENDS Command • A number of applications will have the CONNECT formula be dependent on the APOS from multiple axes. • For safety purposes, it is best that the controller is told about these dependencies, which is done using the DEPENDS command. • When an axis is dependent on another axis, the firmware has the following behavior: • When initiating a motion the firmware will verify if each axis dependent on it is enabled, and if any are disabled the motion will not start • If in the process of motion a motor is disabled or killed due to a fault or DISABLE or KILL command, the firmware will immediately HALT all motions involving axes the motor depends on • If a SET command is executed on an axis the motor depends on, the firmware will recalculate ROFFS to prevent an instantaneous jump in the motor

  27. DEPENDSCommand – Example 1 • A CONNECT formula is defined on the right that is the summation of two profiles: APOS(X) + APOS(Y) • If axis X is disabled and a motion is commanded on axis Y, will the motion start? • If axis Y is jogging and axis X motion gets killed due to a limit switch, what happens to axis Y? • If axis Y is homed and the feedback position zeroed using the SETFPOS command, what happens to X? ! Variable Declaration globalintX, Y ! Variable initialization X = 0; Y = 1 ! Toggle default CONNECT bit MFLAGS(X).#DEFCON = 1 MFLAGS(X).#DEFCON = 0 ! Set non-default CONNECT formula CONNECTRPOS(X) = APOS(X) + APOS(Y) DEPENDS(X), (X, Y) STOP

  28. ACSPL+ Programming Example: 1 An encoder tape is poorly mounted giving an Abbe error of 2.5⁰ as shown in the figure below. Write a program that uses the CONNECT command to correct for the error. • If done properly, when the user commands a 1 meter move, the encoder will read a distance of 1.001 meter. • The program should be able to be executed at any position, and ROFFS should always be 0

  29. ACSPL+ Programming Example: 2 A 2D error map was measured for the X axis of an X-Y table between positions (-180,-180) and (180, 180). The data is provided in ‘Programming XX – CONNECT Command Example 2.prg’. Finish the program by implementing the CONNECT command. • The program should be able to be executed at any position, and ROFFS should always be 0

More Related