AN2350 datasheet(32/41 Pages) STMICROELECTRONICS

32 / 41 page

Control algorithm

AN2350

32/41

As shown in the figure above, the trajectory in this case is made of three tracts.

●

A straight-line tract with a trapezoidal velocity profile that brings the end-effector from

the start point

to the origin

●

A change of the configuration in which the entire arm is rotated of 180°. We can see

how the position of the end-effector remains the same while its orientation is changed.

●

A straight-line tract with a trapezoidal velocity profile that brings the end-effector from

the origin

to the stop point

Figure 25.

Motion path planning scheme to avoid singularities

4.2

PID position control algorithm

The points of the trajectory table have to be given as input one by one to the control

algorithm every TR , that has been fixed in 20 ms, while the PID control interrupt service

routine (ISR) is performed every 1ms.

The trajectory table is composed of two arrays; one for the shoulder (shoulder_array[ ]) and

one for the elbow (elbow_array[ ]), obviously of the same dimension.

The control ISR has been associated to the compare unit CC2, while the scanning of the

trajectory table has been associated to the compare unit CC3.

The incremental encoders of the shoulder and the elbow have been associated to the GPT1

timers T2 and T4, respectively, used in incremental interface mode.

)

(

1 y

)

(

)

(

)

(

Start point (x1, y1)

Stop point (x2, y2)

c=0

Motion path planning

Straight-line motion

(x1,y1)

⇒ (x2,y2)

Trajectory table as

encoder pulses

Straight-line motion

(x1,y1)

⇒ (0,0)

Configuration change

180° rotation

Straight-line motion

(0,0)

⇒ (x2, y2)

sign(x1)

sign(x2)

AN2350 bảng dữ liệu(PDF) 32 Page - STMicroelectronics

AN2350 bảng dữ liệu(HTML) 32 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

bảng dữ liệu tải về

Link URL