AN2350 datasheet(30/41 Pages) STMICROELECTRONICS

30 / 41 page

Control algorithm

AN2350

30/41

Figure 22.

Convergent loop procedure for a trapezoidal velocity profile

At the generic sampling time Ti , the covered distance, accordingly to the previous equation,

is equal to

; so the point

of the trajectory to reach in a straight-line motion

at time Ti in the workspace, can be obtained resolving the following system:

Knowing

it is possible to obtain the corresponding joint variables

1 and α2 with the

inverse kinematics equations. These joint variables must be opportunely converted in

incremental encoder pulses, because the position closed loop control is performed on the

feedback of the incremental encoders of the two joints.

During the trajectory planning it is very important that the manipulator avoids crossing the

kinematics singularities of the accessible workspace since they could make the continuation

of the trajectory impossible.

For this wafer handler, these singularities are distributed along the circle with radius 51 mm

and center in the origin of the base coordinates. The motion singularity appears only when

the manipulator, during the motion, crosses the point of the circle assuming a configuration

with the arms aligned, as shown in the following figure.

Acceleration a

Cruising Speed vc

Max Distance Smax

t1 ≥ t3 / 2

t1 = vc / a

t3 = t1 + smax / vc

t1 = t3 / 2 - TR

vc = t1 . a

t3 = t1 + smax / vc

)

(

)

(

⎩

⎨

⎧

−

)

(

)

(

)

(

AN2350 Datasheet(PDF) 30 Page - STMicroelectronics

AN2350 Datasheet(HTML) 30 Page - STMicroelectronics

Html Pages

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41

Datasheet Download

Link URL