AN2350 datasheet(35/41 Pages) STMICROELECTRONICS

35 / 41 page

Control algorithm

AN2350

35/41

Figure 27.

Trajectory table scanning ISR

To avoid oscillations due to external events that could change the motion direction, an

additional control algorithm is performed using the ISR associated to timer T6, every 200ms.

This implies that if for 10 times no new reference point is given to the control ISR, in the ISR

associated to timer T6 the values of PWM are strongly limited until the stop point is reached,

drastically reducing the oscillations.

4.3

Homing procedure

The Homing procedure is the most important task a manipulator must execute at start-up. It

can be seen as a calibration and alignment phase to the end of which, the manipulator is in

a configuration that allows it to perform the other tasks correctly.

For the wafer handler there are many possible home alignment configurations according to

the position in which it is placed and to the tasks that it must execute.

For our purpose a homing position has been chosen in which the controlled arm is

completely extended. This configuration corresponds to an end effector position at the point

of coordinates (481,0).

The homing procedure has been performed through the absolute encoder feedback of the

two links, using three GPIOs (one for each bit of the absolute encoder) and one Capture pin

(to capture every transition in each bit), for each link. The homing procedure is performed

first for the shoulder and then for the elbow. The figures below show the value of the

absolute encoder for the shoulder and for the elbow.

Reload of compare unit CC3

index++;

sh_pulses_desired=shoulder_array[index];

elb_pulses_desired=elbox_array[index];

if(flag_error)

Exit

Stop and Reset Timer T6

Redefine upper and lower PWM limit

(min PWM=10% - maw PWM=90%)

Start Timer T6

AN2350 датащи(PDF) 35 Page - STMicroelectronics