AN2350
Application note
Guidelines for the control of a multiaxial planar robot
with ST10F276
Introduction
This application note describes how to implement a PID control with the ST10F276 16-bit
microcontroller for the control of a multiaxial planar robot.
The document provides guidelines for the complete development of a control system, able
to fulfill all the requirements needed to drive an industrial manipulator.
The first chapter is an introduction to the robotic manipulators. It focuses on their working
space, forward kinematics and the problem of the inverse kinematics. In particular it
describes the main characteristics of an industrial wafer handler used as a case study for a
multiaxial planar manipulator family.
The second chapter is a brief description of the ST10F276 16-bit microcontroller with a
focus on its architecture and its peripherals. Moreover, an overview is given of the control
board, named Starter Development Kit - ST10F276 and its three dedicated connectors for
motion control.
The third chapter provides an overview of the hardware and mechanical equipment of a
wafer handler. More specifically, it describes the encoder conditioning and motor driver
circuits.
The fourth chapter is dedicated to the description of the basic routines for implementing PID
control. The inverse kinematics of the wafer handler and the planning of the trajectory are
also explained. The implementation of the teach and repeat technique and the homing
procedure are shown.
See associated datasheets and technical literature for details of the components related to
the devices and board used in this application note:
http://www.st.com/stonline/books/ascii/docs/9944.htm (L6205 Product Page)
November 2006
Rev 1
1/41
www.st.com
Contents
AN2350
Contents
1
An overview on robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1
1.2
2
2.2
4
5
2/41
1.1.1
Kinematics analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.1.2
Singularity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
The industrial wafer handler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2.1
Forward kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.2.2
Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
The SDK-ST10F276 control board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1
3
Structure of a manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
Brief description of the SDK-ST10F276 . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1.1
User Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.2
On board motor control connectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
ST10F276 16-bit microcontroller - architectural overview . . . . . . . . . . . . 14
2.2.1
Basic CPU concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2.2
Memory organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.3
On-chip peripheral blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.4
Managing Interrupts (hardware) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
Hardware and mechanical equipments . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.1
The Dual DC motor and the power stage . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2
Cables and connectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.3
The encoders and the conditioning circuit . . . . . . . . . . . . . . . . . . . . . . . . 20
3.4
Schematics of the driver board and the interface board. . . . . . . . . . . . . . 23
Control algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.1
Motion and path planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.2
PID position control algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.3
Homing procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.4
Teach and Repeat procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
Revision history . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
AN2350
An overview on robotics
1
An overview on robotics
1.1
Structure of a manipulator
A manipulator, from a mechanical point of view, can be seen as an open kinematic chain
constituted of rigid bodies (links) connected in cascade by revolute or prismatic joints, which
represent the degrees of mobility of the structure. These manipulators are also known as
serial manipulators.
Only relatively few commercial robots are composed of a closed kinematic chain (parallel)
structure. In this case there is a sequence of links that realize a loop. From this point on, we
refer only to serial manipulators.
In the chain mentioned above, it is possible to identify two end-points: one end-point is
referred to as the base, and it is normally fixed to ground, the other end-point of the chain is
named the end-effector and is the functional part of the robot. The structure of an end
effector, and the nature of the programming and hardware that drives it, depends on the
intended task.
The overall motion of the structure is realized through a composition of elementary motions
of each link respect to previous one.
A revolute joint allows a relative rotation about a single axis, and a prismatic joint permits a
linear motion along a single axis, namely an extension or retraction.
It is assumed throughout that all joints have only a single degree-of-freedom: the angle of
rotation in the case of a revolute joint, and the amount of linear displacement in the case of
a prismatic joint.
The degrees of mobility must be suitably distributed along the mechanical structure in order
to furnish the needed degrees of freedom (DOF) to execute a task. If there are more
degrees of mobility than degrees of freedom the manipulator is said to be redundant.
The workspace of a point H of the end-effector is the set of all points which H occupies as
the joint variables are varied through their entire ranges. The point H is usually chosen as
either the center of the end-effector, or the tip of a finger, or the end of the manipulator itself.
The workspace is also called work volume or work envelope.
Size and shape of the workspace depend on the coordinate geometry of the robot arm, and
also on the number of degrees of freedom.
The workspace of a robot is a fundamental criterion in evaluating manipulator geometries.
Manipulator workspace may be described in terms of the dexterous workspace and the
accessible workspace. Dexterous workspace is the volume of space which the robot can
reach with all orientations. That is, at each point in the dexterous workspace, the endeffector can be arbitrarily oriented.
The accessible workspace is the volume of space which the robot can reach in at least one
orientation. In the dexterous workspace the robot has complete manipulative capability.
However, in the accessible workspace, the manipulator's operational capacity is limited
because of the terminal device can only be placed in a restricted range of orientations.
In other words, the dexterous workspace is a subset of the accessible workspace.
Table 1 shows a classification of the manipulators accordingly to the type and sequence of
the degrees of mobility of the structure, and of their workspaces.
3/41
An overview on robotics
Table 1.
Open chain manipulators classification
Type
Workspace
Joints
Cartesian
– Three prismatic joints
– A Cartesian degree of
freedom corresponds to every
joint
Cylindrical
– One revolute joint and two
prismatic joints
– Cylindrical coordinates
Spherical
– Two revolute joints and one
prismatic joint
– Spherical coordinates
SCARA
– Two revolute joints and one
prismatic joint
– Selective Compliance
Assembly Robot Arm
Anthropomorphic
4/41
AN2350
– Three revolute joints
– It is the most dexterous
structure
AN2350
1.1.1
An overview on robotics
Kinematics analysis
Robot arm kinematics deals with the analytical study of the geometry of motion of a robot
arm with respect to a fixed reference coordinate system as a function of time without regard
to the forces/moments that causes the motion.
Thus, it deals with the analytical description of the spatial displacement of the robot as a
function of time, in particular the relations between the joint space and the position and
orientation of the end-effector of a robot arm.
In kinematics, we consider two issues:
1.
Forward analysis: for a given manipulator, given the joint angle vector
q(t)=(q1(t), q2(t), …., qN(t))T
and the geometric link parameters, where n is the number of degrees of freedom,
what is the position and orientation of the end-effector with respect to a reference
coordinate system?
2.
Inverse analysis: given a desired position and orientation of the end effector and the
geometric link parameters with respect to a reference coordinate system, can the
manipulator reach the desired manipulator hand position and orientation. And if it can,
how many different manipulator configurations will satisfy the same condition?
For serial robots, the forward analysis problem is usually easy and straightforward.
Unfortunately, the inverse analysis problem is of much more interest. For example, in
industrial applications, the end-effector must follow some desired path; then, we need to find
the joint angles for each position in the path.
For redundant robots, the inverse kinematics problem has then an infinite number of
solutions. The extra degrees of freedom can then be used for other purposes, for example
for fault tolerance, obstacle avoidance, or to optimize some performance criteria.
A simple block diagram indicating the relationship between these two problems is shown in
the following figure.
Figure 1.
The direct and inverse kinematics problems
Link parameters
Joint angles
q1(t), q2(t), ..., qN(t)
Direct
kinematics
Position and
orientation
of the end-effector
Link parameters
Joint angles
q1(t), q2(t), ..., qN(t)
Inverse
kinematics
5/41
An overview on robotics
AN2350
Since the links of a robot arm may rotate and/or translate with respect to a reference
coordinate frame, the total spatial displacement of the end-effector is due to the angular
rotations and linear translations of the links.
Denavit and Hartenberg proposed a systematic and generalized approach of utilizing matrix
algebra to describe and represent the spatial geometry of the links of a robot arm with
respect to a fixed reference frame. This method uses a 4 x 4 homogeneous transformation
matrix to describe the relationship between two adjacent rigid mechanical links and reduces
the direct kinematics problem to finding an equivalent 4 x 4 homogeneous transformation
matrix that relates the spatial displacement of the hand coordinate frame to the reference
coordinate frame. These homogeneous transformation matrices are also useful in deriving
the dynamic equation of motion of a robot arm.
In general, the inverse kinematics problem can be solved by several techniques. Most
commonly used approaches are matrix algebraic, iterative, or geometric. A geometric
approach based on both the link coordinate systems and the manipulator configuration has
been used for the industrial wafer handler to which this application note refers.
1.1.2
Singularity
A significant issue in kinematic analysis surrounds so-called singular configurations.
Physically, these configurations correspond to situations where the robot joints have been
aligned in such a way that there is at least one direction of motion (the singular direction[s])
for the end effector that physically cannot be achieved by the mechanism. This occurs at
workspace boundaries, and when the axes of two (or more) joints line up and are
redundantly contributing to an end effector motion, at the cost of another end effector DOF
being lost.
1.2
The industrial wafer handler
This section describes the main characteristics of an industrial wafer handler, used as a
case study for a multiaxial planar manipulator family.
The structure of the manipulator consists of two arms with six joints (one prismatic joint and
five revolute joints) arranged in order to have the motion axes parallel to each other.
The illustration below shows the relation between the axis control and the robot
components.
6/41
AN2350
An overview on robotics
Figure 2.
The structure of the wafer handler
Paddle 1
Paddle 0
Link 2
1
Link 1
2
H axis
Link 2
Link 1
A axis
3
Y axis
W axis
5
4
X axis
8
Z axis
6
1. Paddle
2. Link 2
3. Link 1
4. Dual Motor Cover
5. Torso Beam
6. Robot Body Mounting Flange
7. Chassis Skin
8. Z-Tube
7
The robot uses 6 electric motors placed as follows:
1.2.1
●
Z Axis Motor (Vertical elevation)
●
X Axis Motor (Rotation entire robot)
●
Dual Motors (Link 2 and Link 1)
Forward kinematics
Accordingly to the Denavit-Hartenberg convention, an orthonormal cartesian coordinate
system (xi, yi, zi) has been established for each link, in order to characterize the forward
kinematics through a D-H homogeneous transformation matrix.
Figure 3.
The Denavit Hartenberg convention for one arm
y
y4
x4
x2
y2
θ3
y3
θ4
y1
y0
x3
θ2
x0
x1
x
Regarding the base coordinates (x0, y0, z0) , the z0 axis lies along the axis of motion of the
prismatic joint. The origin O0 of the base coordinates has been placed at the same height of
7/41
An overview on robotics
AN2350
the end effector, when the prismatic joint is at its lower level. The x0 axis lies along the
longitudinal direction of the link 1.
Since all the motion axes are parallel to each other, the origins of the other coordinate
frames have been placed at the same height of the origin O0.
The Denavit-Hartenberg parameters are described in the following table:
Table 2.
The Denavit Hartenberg parameters for one arm
Link
ai [mm]
α [rad]
di [mm]
θ [rad]
1
154
0
d1
0
2
133
0
0
2
3
133
0
0
3
4
215
0
0
4 =- 3/2
In this application note we refer only to one arm, without considering the Z Axis (Vertical
elevation - prismatic joint) and X Axis (Rotation entire robot - revolute joint).
Through a motion simulation it has been possible to obtain the workspace of the
manipulator: for each value of the angle of the joint 1, the joint 2 performs a complete
revolution. As shown in the Figure 4, the workspace has a circular form. Nevertheless
because the joint 3 is under-actuated, there are some regions of the workspace
characterized of different orientations of the end-effector.
Figure 4.
8/41
The workspace of the wafer handler
AN2350
An overview on robotics
Out from the circumference, with origin (154 mm, 0 mm) and radius 51 mm, the manipulator
is able to reach a point of the workspace with two possible configurations of the joint
variables; the points inside this circumference are reachable with four possible
configurations of the joint variables as shown in Figure 5.
The center of the workspace is reachable with every orientation.
Figure 5.
1.2.2
A detail of the central area of the wafer handler workspace
Inverse kinematics
A geometric approach based on the link coordinate systems and on the manipulator
configuration has been used for the manipulator. Since we refer to the control of only one
arm (link 1 - shoulder, link 2 - elbow), the origin of the base coordinates has been placed in
such way that the z0 axis lies along the rotation axis of the revolute joint of the shoulder.
First of all, we consider the manipulator in the configuration described in the following figure.
9/41
An overview on robotics
Figure 6.
AN2350
The geometric approach for the inverse kinematics
y
α2 = -2α’1
l1
l2
α’ 1
l3
x1
x3
x2
x
α3 = -α2 / 2 = α’1
There is a mechanical connection between the link 3 and the link 2, which imposes
α
α 3 = − 2 . This implies that the joint 3 can not be actuated independently from the joint 2.
2
Since
l1 = l2 = l
⎧ x1 = l cos α1'
⎪
'
⎨ x2 = x1 + l cos α 2 = 2l cos α1
⎪
'
⎩ x3 = x2 + l3 = 2l cos α1 + l3
From the last equation and from geometric considerations follows:
α1' = arccos
x3 − l3
2l
α 2 = −2α1'
α
α 3 = − 2 = α1'
2
Consider now a rotation of an angle ϕ:
10/41
AN2350
An overview on robotics
Figure 7.
The inverse kinematics after a rotation
y
α2 = -2α’1
l2
l1
α’1
l3
α1
Ζ
ϕ
α3 = -α2 / 2 = α’1
x
Knowing the position of the end-effector in the workspace, it is possible to deduce its
distance from the origin and the angle ϕ:
z = x2 + y2
ϕ = atn
y
x
α1 = ϕ + arccos
z − l3
2l
while α2 and α3 remains the same, because they are independent from the rotation of the
manipulator.
11/41
The SDK-ST10F276 control board
2
The SDK-ST10F276 control board
2.1
Brief description of the SDK-ST10F276
AN2350
The SDK-ST10F276 is a two-layer, low-cost development board with an ST10F276 16-bit
Embedded Flash Memory Microcontroller (see Figure 8). It is considered a general purpose
application board used for developing advanced motor control solutions and processing
external data (e.g., sensor outputs).
Three dedicated connectors for motion control allow the user to control different kinds of
motors by plugging in external power motor boards.
Figure 8.
12/41
The SDK-ST10F276 board
AN2350
2.1.1
The SDK-ST10F276 control board
User Interfaces
The main interfaces used to communicate with the SDK-ST10F276 platform include:
●
Two CAN 2.0B interfaces which operate on one or two CAN buses (30 or 2x15
message objects)
●
One RS232 connector for serial signal communication with external devices
●
One I2C connector for additional external device management
●
Three dedicated connectors for motion control (external power motor boards)
●
A series of general purpose LEDs and DIP switches
●
One potentiometer and a push-button
For the user's convenience, the ST10 I/O pins on the Development board are split into four
main connectors located on each side of the ST10 device
The five general purpose Light Emitting Diodes (LEDs) may be used for end-user
applications.
They can be activated via the JP2 connector, or by using a series of dip switches on the
board.
The potentiometer and push-button may be set in different and independent ways for
general purpose uses.
2.1.2
On board motor control connectors
The SDK-ST10F276 board provides an embedded solution (3 modular connectors) for
power requirements up to 3000W, so they are well-suited for driving motors (e.g. BLDC,
Brushless Direct Current) via an appropriate external motor board. The various power
boards that can be connected to the SDK-ST10F276 are optimized in order to support a
wide variety of advanced motor control applications.
Each of the three power connectors on the SDK-ST10F276 development board has 26 pins
(see Figure 9). When a connector is used with a compatible motor board, the user can drive
motors with the following signals:
●
PWM
●
Hall Effect sensor/GPIO
●
Power/Brake
●
Encoder
●
Tachometer/Resolver
●
Shutdown, and Alarm
13/41
The SDK-ST10F276 control board
Figure 9.
AN2350
One of the 26 pin connectors of the SDK-ST10F276 board
J2
HALL1
HALL3/GPIO1
L1/ENABLE
L2
L3/GPIO4
T2EUD
DINAMO/RES1
EC1
SHUT DOWN
ALARM
1
3
5
7
9
11
13
15
17
19
21
23
25
2
4
6
8
10
12
14
16
18
20
22
24
26
PWM0
HALL2/GPIO2
H1/BRAKE
H2/FW\REV
H3/GPIO3
T2IN
SENSE
RES2
EC2
Set Point ENC
P7.0
P8.1
P1L.0
P1L.2
P1L.4
P3.7
P5.0
P5.2
P2.14
P2.1
+ 12V
1
P8.0
P8.2
P1L.1
P1L.3
P1L.5
P5.15
P5.1
P2.13
P4.0
P4.2
C5
2
GND
100nF
GND
2.2
ST10F276 16-bit microcontroller - architectural overview
The ST10F276 is a high performance 64MHz 16-bit microcontroller with DSP functions.
ST10F276 architecture combines the advantages of both RISC and CISC processors with
an advanced peripheral subsystem. The following block diagram gives an overview of the
different on-chip components and of the advanced, high bandwidth internal bus structure of
the ST10F276. (see Figure 10).
14/41
AN2350
The SDK-ST10F276 control board
Figure 10. ST10F276 functional block diagram
2.2.1
Basic CPU concepts
The main core of the CPU includes a 4-stage instruction pipeline, a 16-bit arithmetic and
logic unit (ALU) and dedicated SFRs.
Several areas of the processor core have been optimized for performance and flexibility.
Functional blocks in the CPU core are controlled by signals from the instruction decode
logic. The core improvements are summarized below:
●
High instruction bandwidth / fast execution
●
High function 8-bit and 16-bit arithmetic and logic unit
●
Extended bit processing and peripheral control
●
High performance branch, call, and loop processing
●
Consistent and optimized instruction formats
●
Programmable multiple priority interrupt structure
15/41
The SDK-ST10F276 control board
2.2.2
AN2350
Memory organization
The memory space of the ST10F276 is organized as a unified memory. Code memory, data
memory, registers and I/O ports are organized within the same linear address space. The
main characteristics are summarized below:
2.2.3
●
512K Bytes of on-chip single voltage Flash memory with Erase/Program controller
●
320K Bytes of on-chip extension single voltage Flash memory with Erase/Program
controller (XFLASH)
●
Up to 100K Erase/Program cycles
●
Up to 16 MB of linear address space for code and data (5 MB with CAN or I2C)
●
2K Bytes of on-chip internal RAM (IRAM)
●
66K Bytes of on-chip extension RAM (XRAM)
On-chip peripheral blocks
The ST10F276 generic peripherals are:
●
Two General Purpose Timer Blocks (GPT1 and GPT2)
●
Serial channel: Two Synchronous/Asynchronous, two High-Speed Synchronous, I2C
standard interface
●
A Watchdog Timer
●
Two 16-channel Capture / Compare units (CAPCOM1 and CAPCOM2)
●
A 4-channel Pulse Width Modulation unit and 4-channel XPWM
●
A 10-bit Analog / Digital Converter
●
Up to 111 General Purpose I/O lines
Each peripheral also contains a set of Special Function Registers (SFRs), which control the
functionality of the peripheral and temporarily store intermediate data results. Each
peripheral has an associated set of status flags. Individually selected clock signals are
generated for each peripheral from binary multiples of the CPU clock.
2.2.4
Note:
16/41
Managing Interrupts (hardware)
●
8-channel Peripheral Event Controller for single cycle, interrupt-driven data transfer
●
16-levels of priority for the Interrupt System with 56 sources, and a sampling rate down
to 15.6ns at 64MHz
Refer to the ST10 information listed in the Introduction on page 1 for device details.
AN2350
Hardware and mechanical equipments
3
Hardware and mechanical equipments
3.1
The Dual DC motor and the power stage
Each arm of the wafer handler is controlled by a dual DC motor (see Figure 17 on page 24);
they are mounted in the same chassis with two armatures which rotate on the same axis
and having concentric drive hubs, as shown in the Figure 12.
Figure 11. The Dual DC motor
The upper armature is connected to the outer drive hub and controls the shoulder (link 1),
while the lower armature is connected to the inner drive hub and controls the elbow (link 2)
through a drive belt with a transmission ratio of 1/2.
17/41
Hardware and mechanical equipments
AN2350
Figure 12. The chassis of the dual DC motor
Each DC motor is driven by a monolithic solution (L6205PD) that is a dual full bridge driver.
In order to increase the current capability, the two full bridges inside the driver have been
connected in parallel mode.
A current controller IC (L6506) has been used for both the monolithic drivers in order to
sense the current motor. When the sensed motor current exceeds its current rate, the
current controller disables the power drivers.
Each motor is driven through a PWM signal generated by the microcontroller; the velocity
and the direction of rotation of each DC motor depends on the duty cycle of such signal. For
duty cycles over 50%, the motor moves in one direction, while for duty cycles under 50%,
the motor moves in the opposite direction. For a duty cycle of 50% the motor is stopped.
The driver board has been developed in order to control three links, even if the control
algorithm refers to the control of only one arm (two links).
In the driver board there is also placed a DC/DC converter that generates all the needed
voltages to supply all the circuits and the SDK-ST10F276 board. A detail of the schematic of
the power stage is shown in the following figure.
18/41
AN2350
Hardware and mechanical equipments
Figure 13. A detail of the power stage
19/41
Hardware and mechanical equipments
3.2
AN2350
Cables and connectors
On the chassis skin of the wafer handler are present six 15 pin D-SUB male connectors.
Four of them (named A, W, Y, H) are used for the links of the two arms, while the other two,
named X and Z, are used respectively for the torso beam and the Z elevation. The four
harness connectors of the links are located under the torso beam.
The wafer handler is furnished of cables, each of one presents on one side a 15 pin D-SUB
female connector, and on the other side a 25 pin D-SUB female connector. The former is
connected to the wafer handler, while the latter is connected to the driver board. The table
below shows the pins assignment and the related description.
Table 3.
3.3
The sectors of the absolute encoder
Pin DB15 Connector
Pin DB25 Connector
Wire
Description
1
1
Green
Incremental encoder
(sin)
2
2
Orange
Incremental encoder
(cos)
3
3
Violet
4
16
Not used
5
9
White
Absolute encoder
(bit b2)
6
10
Grey
Absolute encoder
(bit b0)
7
7
Not used
8
8
Red
9
14
Yellow
VREF-
10
15
Blue
VREF+
11
19
Not used
12
21
Black
13
23
Not used
14
24
Black (motor)
Motor +
15
25
White (motor)
Motor -
Absolute encoder
(bit b1)
2.5V supply for encoder
read head
Ground for encoder
read head
The encoders and the conditioning circuit
Each joint of the arm of the manipulator has an encoder ring as shown in Figure 14. In this
ring it is possible to identify four sector named A, B, C and D, and the encoder read head.
20/41
AN2350
Hardware and mechanical equipments
Figure 14. The encoder ring of one DC motor
The sectors D, A and B represent the absolute encoder, while the sector C represents the
incremental encoder.
The opaque parts of each sector of the absolute encoder are placed according to a Gray
code, in order to offer absolute position of the link with a resolution of 45 degrees. In this way
it is possible to identify 8 angular positions accordingly to the scheme below, where the
sector D (white wire) represents the most significant bit of the encoder, while the sector A
(grey wire) represents the least significant bit.
Table 4.
The sectors of the absolute encoder
Angular position
Sector D
(white wire)
b2
Sector B
(Violet wire)
b1
Sector A
(Grey wire)
b0
Absolute
encoder values
b2b1b0
1
0
0
0
0
2
0
0
1
1
3
0
1
1
3
4
0
1
0
2
5
1
1
0
6
6
1
1
1
7
7
1
0
1
5
8
1
0
0
4
In order to capture every transition, in each bit of the absolute encoder has been developed
a logic circuit with NAND ports. A detail of the conditioning circuit for one of the absolute
encoders of the wafer handler is shown in Figure 15. In this way the output of this circuit can
be associated to an external interrupt of the microcontroller, while the output bits of the
absolute encoder can be associated to three GPIOs (General Purpose Input Output) in
order to read its value.
21/41
Hardware and mechanical equipments
AN2350
Figure 15. The conditioning circuit for one absolute encoder
The sector C represents the incremental 12-bit encoder, whose outputs are two sinusoidal
signals (available in orange and green wires) shifted to each other by 90 degrees, with an
offset of 2.5 V and a peak-to-peak voltage of 1.1 V.
Since these signals can not be directly managed by the microcontroller, it has been
necessary to develop a conditioning circuit whose outputs are digital signals.
This conditioning circuit for each sinusoidal signal consists of a comparator whose output is
at a logical high level (5 V) every time the input signal level is over 2.5 V, and at logical low
level (0 V) every time the level of the input signal is below 2.5 V. A detail of the conditioning
circuit for one of the incremental encoders of the wafer handler is shown in the following
figure.
22/41
AN2350
Hardware and mechanical equipments
Figure 16. The conditioning circuit for one incremental encoder
Using a trimmer it is possible to have a square wave with a duty cycle of 50% as output.
In this way from the two input sinusoidal signals it is possible to obtain two square signals
shifted each other of 90 degrees, through which it is possible to count 4096 pulses for each
rotation of the joint.
The conditioning circuit presents also a D flip-flop through which it is possible to obtain the
direction of rotation of the joint.
3.4
Schematics of the driver board and the interface board.
The driver board presents three 25 pin D-SUB male connectors to which are plugged the
cables of the wafer handler. In the same board there are 26 pin male connectors, used to
interface the SDK-ST10F276 board through three flat cables.
Figure 17 and Figure 18 show the schematic of the driver board.
23/41
Hardware and mechanical equipments
AN2350
Figure 17. Driver board schematic (page 1)
5
4
3
+5V
+5V
5
A_1
10K
motor+_1 2
CK GND
Q
4 dir_rot_1
white_1
1
74V1G79
R5_1
12k
+5V
violet_1
3
1
3
5
7
9
11
13
15
17
19
21
23
25
+5V
motor-_2
2
4
6
8
10
12
14
16
18
20
22
24
J2
5
orange_2
orange_1
4
C5_1
100nF
+2.5V
grey_2
+
LM339
-
+5V
2
3
1
2
3
4
5
A_1
B_1
dir_rot_1
4
U3B
74HC14
11
bit2_1_
1 bit1_1_A
grey_1
5
6
U7C
74HC14
9
C8_1 100nF
3
4
5
U10B
74HCT10
1
2
bit0_1_
8
12k
4
5
bit2_1
D
6
1 bit0_1_A
U7D
74HC14
bit0_1
12
+5V
bit1_1
bit0_1
JP0_1
2
R7_1
U10A
74HCT10
bit1_1_
U7E
74HC14
bit1_1
1
2
13
JP1_1
10
R6_1
12k
J7
4.7k
C11_1
22nF
C8_0
470uF/6.3V
1 bit2_1_A
12
U7F
74HC14
4
U7B
74HC14
R4_1
1
13
bit2_1
2
R1_1
+2.5V
green_2
violet_2
white_2
2
U7A
74HC14
3
14
14
D
2
2
C13_0
JP2_1
2
1
R1
1.6M
U1A
C5_0
470uF/6.3V
6
U3C
74HC14
CON25A
C12_0
U4
1
1
C10_1
22nF
5
14
U1B
+
LM339
-
7
6
7
7
green_1
C1_1
100nF
+5V
C7_1 100nF
Vcc
+2.5V
C7_0
470nF/6.3V
1
1
C6_1 100nF
14
motor-_1
+2.5V
grey_1
C4_1 100nF
9
10
11
U10C
74HCT10
8
1
2
13
U11A
74HCT10
12
J_J7
U14A
74HCT20
6
1 abs_enc_1
2
7
D
orange_1
C3_1 100nF
2
7
C4_0
2
4
6
8
10
12
14
16
18
20
22
24
J1
3
1
3
5
7
9
11
13
15
17
19
21
23
25
+5V
R2_1
4.7k
12
green_1
violet_1
white_1
2
+5V
+5V
100nF C2_1
C10_0 C11_0
+5V
C9_1 100uF/6.3V
25 pin D-SUB male connectors
CON5
10K
R3_1
1
R2
1.6M
motor+_2
2
CON25A
+5V
motor-_3
+5V
C6_2 100nF
green_2
1
C1_2
100nF
R3
1.6M
motor+_3
+
LM339
8 -
+5V
14
9
8
U3D
74HC14
C8_2
22nF
Vcc
2
A_2 1
2
D
CK GND
4 dir_rot_2
white_2
1
GND
R5_2
12k
3
GND
VINVOUT
D1_0
C2_0
100nF C3_0
470uF/6.3V
LED
+
LM339
-
10
C5_2
100nF
11
+5V
1
2
3
4
5
A_2
B_2
dir_rot_2
10
U3E
74HC14
C9_2
22nF
grey_2
1 bit1_2_A
U9E
74HC14
6
9
JP0_2
R7_2
12k
bit1_2
bit0_2
9
10
11
U11C
74HCT10
8
bit2_2
1
2
13
U12A
74HCT10
12
3
4
5
U12B
74HCT10
6
bit0_2_
12
13
U9D
74HC14
R0
470
dir_rot_1 1
2
GND
+5V
+5V
C4_3 100nF
green_3
1
2
U3A
74HC14
C10_3
22nF
14
5
CK GND
3
+2.5V
2
4
6
8
10
12
14
16
18
20
22
24
26
1
5
+
LM393
6 -
orange_3
C5_3
100nF
7
C11_3
22nF
13
2
74HC14
U8F
13
bit2_3_
3
4
74HC14
U8E
11
U3F
12
74HC14
A_3
B_3
dir_rot_3
1
2
3
4
5
grey_3
R7_3
12k
U8D
6
74HC14
U12C
74HCT10
8
9
+5V
C8_3 100nF
bit1_3_
bit1_3
bit0_3
JP0_3
U8C
5
1 bit1_3_A
2
10
74HC14
bit1_3
R6_3
12k
9
10
11
JP1_3
U8B
violet_3
J9
B
1 bit2_3_A
2
12
74HC14
bit2_3
R5_3
12k
+5V
4.7k
U2B
1
2
13
U13A
74HCT10
12
1
2
1 bit0_3_A
2
bit0_3_
8
74HC14
4
5
bit0_3
bit2_3
3
4
5
U13B
74HCT10
6
J_J9
U15A
74HCT20
6
1 abs_enc_3
2
CON5
+5V
C9_3 100nF
10K
R3_3
14
Motor_2 ARM_1
Q
1
2
J5
GND
D
white_3
74V1G79
R4_3
1
3
5
7
9
11
13
15
17
19
21
23
25
2
JP2_3
U8A
4 dir_rot_3
+5V
GND
bit0_2_A
abs_enc_2
IN3
IN3
bit2_2_A
bit1_2_A
EN_B
EN_B
bit0_2_
bit1_2_
bit2_2_
J_J3 A_2
B_2
1
2
+5V
J_J4
dir_rot_2 1
2
A_3 1
10K
R1_3
CON26A
+5V
C7_3 100nF
U6
1
1
7
+
LM393
2 -
C1_3
100nF
C6_3 100nF
R2_3
4.7k
U2A
3
Vcc
8
+2.5V
+5V
2
100nF C2_3
2
4
6
8
10
12
14
16
18
20
22
24
26
4
Motor_1 ARM_1
B_1
J4
1
3
5
7
9
11
13
15
17
19
21
23
25
14
+5V
C12_3 100uF/6.3V
bit0_1_A
abs_enc_1
IN1
bit2_1_A
bit1_1_A
EN_A
bit0_1_
bit1_1_
bit2_1_
J_J1 A_1
1
2
+5V
J_J2
1 abs_enc_2
2
7
To the interface board
IN1
8
CON5
1
EN_A
J_J8
U14B
74HCT20
R3_2
2
2
B
C
9
10
1 bit0_2_A
8
U9C
74HC14 bit0_2
6
bit1_2_
2
5
U11B
74HCT10
JP1_2
10
R6_2
12k
J8
4.7k
1
13 11
3
4
5
10K
1
C1_0
100uF
11
orange_2
bit2_2_
U9F
74HC14
4
U9B
74HC14 bit1_2
R4_2
+2.5V
+5V
+2.5V
2
1 bit2_2_A
12
2
violet_2
2
U1D
U0
LD25
3
13
U9A
74HC14 bit2_2
3
74V1G79
+5V
+2.5V
+5V
2
10K
R1_2
CON25A
+5V
JP2_2
2
Q
14
U5
1
9
14
U1C
+2.5V
+5V
C7_2 100nF
R2_2
4.7k
C9_0
470uF/6.3V
14
+5V
C4_2 100nF
2
+2.5V
grey_3
7
C6_0
470uF/6.3V
+5V
orange_3
7
C
2
4
6
8
10
12
14
16
18
20
22
24
J3
7
1
3
5
7
9
11
13
15
17
19
21
23
25
white_3
5
green_3
violet_3
C15_0
Vcc
C14_0
9
10
11
U13C
74HCT10
9
10
8
12
13
CON26A
U15B
74HCT20
8
7
GND
J6
1
3
5
7
9
11
13
15
17
19
21
23
25
GND
Motor_1 ARM_2
A
bit0_3_A
abs_enc_3
IN5
IN5
bit2_3_A
bit1_3_A
EN_C
EN_C
bit0_3_
bit1_3_
bit2_3_
J_J5 A_3
B_3
1
2
+5V
J_J6
dir_rot_3 1
2
A
2
4
6
8
10
12
14
16
18
20
22
24
26
Title
Size
GND
5
24/41
WAFER HANDLER DRIVER BOARD
Document Number
Rev
CON26A
Date:
4
3
2
Sheet
Friday, March 31, 2006
1
1
of
2
Hardware and mechanical equipments
AN2350
Figure 18. Driver board schematic (page 2)
5
4
3
2
1
+5V
C10_A
C4_A
R1_A
10k 5% 0.25W SMD 1026
C1_A 100nF SMD 1026
R7_A
Kemet Electronics 220nF/100V CER P5
100nF SMD 1026
C5_A
Siemens Matsushita 220nF/100V POLIEST
100 ohm SMD 1026
1N4148 D1_A 1N4148 D2_A
IN4
5
4
74V2G14
R/C
3
Sync
NC
NC
Osc_Out
Vsense1
12
SENSE_A
Vsense2
17
SENSE_B
Vref1
18
Vref2
19
9
Spectrol74W 50k
47uF/6.3V
2
19
VSA
VSB
4
VCP
L6205PD
U18
R12_A
SENSE_A
1k 1%
C8_A
470pF SMD 1026
C9_A
R2_A
20k 1% SMD 1026
+5V
17
1
10
11
20
+5V
IN1A
IN2A
IN1B
IN2B
4.7k 5% 0.25W SMD 1026
5 ENA
R6_A
16 ENB
EN_A
+5V
R0_A
12k 0.25W
R5_A
VBOOT
10
11
20
1
2
+5V
L6506
EN
6
7
14
15
10k 5% 0.25W SMD 1026
OUT1A
OUT2A
9
3
OUT1B
OUT2B
12
18
motor+_1
motor-_1
motor+_1
motor-_1
D
SENSEA
SENSEB
3
CON4
8
13
IN3
4
16
15
14
13
R8_A
U16B
Out1
Out2
Out3
Out4
R6_D
IN3
84
0.22 Ohm 2W 1%
t=150us
+24V
R2_B
20k 1% SMD 1026
R4_A
R3_A
C3_A
CW
t=150us
CW
68nF SMD 1026
1N4148 D1_B 1N4148 D2_B
R3_B
C3_B
Spectrol74W 5k
CW
C2_A
C4_B
R7_B
Kemet Electronics 220nF/100V CER P5
CW
C5_B
68nF SMD 1026
Siemens Matsushita 220nF/100V POLIEST
2.2nF P5
100 ohm SMD 1026
D
In1
In2
In3
In4
GND
5
6
7
8
VCC
IN2
7
74V2G14
GND
GND
GND
GND
8
U17
1
Siemens Matsushita 10nF/100V CER
50 mils
1
2
3
4
U16A
IN1
470uF/35V
C6_A
J10
IN1
C7_A
C7_B
470uF/35V
C6_B
2
Sanyo
Poscap
2
2
19
VSA
VSB
4
VCP
U20
9
3
OUT1B
OUT2B
12
18
motor+_2
motor-_2
motor+_2
motor-_2
SENSEA
SENSEB
OUT1A
OUT2A
C9_B
47uF/6.3V
R12_B
SENSE_B
1k 1%
R8_B
0.22 Ohm 2W 1%
C8_B
470pF SMD 1026
2
2
R6
4K7
C
L6205PD
1
7
6
C19
C20
C16
100uF/6.3V
100uF/6.3V
100nF
R5
1K8
12
1
C17
22nF
2
C18
220pF
EN_B
C15
100uF/6.3V
+
1
COMP
R4
5K6
D0
STPS2L25U
IN1A
IN2A
IN1B
IN2B
4.7k 5% 0.25W SMD 1026
5 ENA
R6_B
16 ENB
8
13
5
1
4
GND
2
10uF
R5_B
R6_E
1
FB
1
VOUT
2
SYNC
+5V
1
INH
2
VREF
C14
+
10k 5% 0.25W SMD 1026
JP1
22uH
2
2
VIN
3
1
8
L1
1
1
U22
L5973D
VBOOT
6
7
14
15
+5V
+5V
+24V
GND
GND
GND
GND
Spectrol74W 5k
C
17
1
10
11
20
Siemens Matsushita 10nF/100V CER
GND
C4_C
R7_C
+5V
C5_C
Kemet Electronics 220nF/100V CER
R1_C
10k 5% 0.25W SMD 1026
C1_C 100nF SMD 1026
Siemens Matsushita 220nF/100V POLIEST
C7_C
470uF/35V
C6_C
IN5
Spectrol74W 50k
R4_C
1
12
Vsense2
17
Vref1
18
Vref2
19
Sync
2
Osc_Out
R5_C
+5V
EN_C
SENSE_C
IN1A
IN2A
IN1B
IN2B
2
19
U21
C9_C
CW
R12_C
SENSE_C
1k 1%
C8_C
470pF SMD 1026
R2_C
20k 1% SMD 1026
B
VSA
VSB
4
L6205PD
4.7k 5% 0.25W SMD 1026
5 ENA
R6_C
16 ENB
47uF/6.3V
+5V
9
3
1
10
11
20
10k 5% 0.25W SMD 1026
L6506
R/C
6
7
14
15
VCP
20
10
11
Vsense1
CON2
VBOOT
EN
16
15
14
13
GND
GND
GND
GND
4
Out1
Out2
Out3
Out4
OUT1A
OUT2A
9
3
OUT1B
OUT2B
12
18
motor+_3
motor-_3
motor+_3
motor-_3
SENSEA
SENSEB
R0_C
12k 0.25W
In1
In2
In3
In4
8
13
74V2G14
+5V
5
6
7
8
R8_C
IN6
R6_F
2
GND
6
passo 50 mils
1
2
NC
NC
U16C
VCC
U19
IN5
17
Siemens Matsushita 10nF/100V CER
J11
B
100 ohm SMD 1026
1N4148 D1_C 1N4148 D2_C
0.22 Ohm 2W 1%
t=150us
CW
C13
R3_C
C3_C
2.2nF P5
+5V
CN1
Vcc
4
Vin
2
1
Spectrol74W 5k
3
D2
POWER SUPPLY-24VDC
K1
6
1N4007
GND
5
8
CN2
+24V
7
P2
A
N.C.
Contact N.A.
DB9 female
connector
RELAY DPDT
+24V
1
6
2
7
3
8
4
9
5
2
1
AUXILIARY-24VDC
1
2
A
+24V
D1
LED
1
R8
2
47K
1 R9
2
M1
2
C12
R7
1K
2.2K
1
Panasonic FA 100uF/63V P5
5
4
3
2
1
To avoid electromagnetic interference, all the signals have been shielded. For this reason an
interface board with three 26 pin female shielded connectors has been developed, in order
to be plugged in the male connectors of the SDK-ST10F276 board. The schematic of the
interface board is shown below.
25/41
Hardware and mechanical equipments
Figure 19. The interface board schematic
26/41
AN2350
Control algorithm
4
Control algorithm
4.1
Motion and path planning
AN2350
Path planning problems generally involve computing a continuous sequence (a path) of
configurations (generalized coordinates) between an initial configuration (start) and a final
configuration (goal) respecting certain constraints.
The term motion planning is usually distinguished from path planning in that the computed
path is parameterized by time (i.e. a trajectory). The consideration of time is often important
for problems requiring time-parameterized solution trajectories.
The aim of the motion planning is to generate the inputs of the motion control system in
order to ensure the correct execution of the planned trajectory by the manipulator.
The path planning can be realized in the joint space or in the workspace. The latter has
been used in this work according to the scheme of the following figure.
Figure 20. General scheme for the path planning
As can be seen, all the reference points necessary to the control of the desired trajectory
are calculated off-line. The trajectory planning algorithm generates a temporal sequence of
variables that specify the position and orientation of the end-effector in the workspace.
Because the control of the manipulator is realized as control of the joints, from this
sequence of variables it is necessary to perform the kinematics inversion to build a
sequence of variables in the joint space. 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.
The path planning method used in this work is the straight-line motion. In this method the
end-effector of the robot travels in a straight line between the start and stop points, using a
trapezoidal velocity profile, in which the motion controller must command the motor driver to
27/41
Control algorithm
AN2350
gradually ramp-up the motor velocity, until it reaches the desired speed and then, gradually
ramp-down it until it stops after the task is complete, as shown in Figure 21:
Figure 21. Example of trapezoidal velocity profile
Motor
Speed
Accelerate
Cruising speed
Decelerate
Stop
Start
t1
t2
Time
t3
The trapezoidal velocity profile is analytically expressed as follows:
0 ≤ t < t1
⎧at
⎪
v(t ) = ⎨vc
⎪v − a (t − t + t )
3
1
⎩ c
t1 ≤ t < (t3 − t1 )
(t 3 − t1 ) ≤ t < t3
where t1 is the time slot for the ramp-up, t3 is the total time for the execution of the entire
trajectory, and t 2 = (t3 − t1 ) is the time slot during which the end effector moves at desired
speed vc (also called the cruising speed).
Integrating it respect to the time, it is possible to obtain the function that describes the space
covered in the workspace.
⎧1 2
⎪ 2 at
⎪
1
⎪
s (t ) = ⎨vc (t - t1 ) + at 2
2
⎪
1 2
1
⎪
2
⎪vc (t3 − 2t1 ) + 2 at1 + vc (t − t3 + t1 ) − 2 a(t - t 3 + t1 )
⎩
0 ≤ t < t1
t1 ≤ t < (t3 − t1 )
(t 3 − t1 ) ≤ t < t3
The time t1 can be obtained from the knowledge of the cruising speed and the acceleration
a (that represent the known parameters)
t1 =
vc
a
and the total time t3 can be obtained from the distance smax between the start point and the
stop point as follows:
s max = s (t3 ) = vc t3 − vc t1
28/41
⇒ t3 =
smax
+ t1
vc
Control algorithm
AN2350
Now follows the algorithm used for the motion planning in order to build a trajectory table
whose points (as joint variables after the reversal kinematics and expressed as incremental
encoder pulses) are given as inputs to the PID control interrupt service routine every TR ,
(fixed in 20 ms).
Let ( x1 , y1 ) the coordinates of the start point and ( x2 , y2 ) the coordinates of the stop
point, vc the cruising speed and a the acceleration. From the previous equations it is
possible to obtain the time t1 and the time t3 , where:
smax = ( x2 − x1 ) 2 + ( y2 − y1 ) 2
y = mx + c as the generic straight-line that intersect the two points ( x1 , y1 ) and
( x2 , y2 ) , where
y −y
m= 2 1
x2 − x1
Consider
is the angular coefficient and
1⎛
( y − y )( x + x ) ⎞
c = ⎜⎜ y1 + y2 − 2 1 2 1 ⎟⎟
2⎝
( x2 − x1 )
⎠
is the intersection with the Y axis.
It is possible that the cruising speed it is not compatible with the imposed acceleration and
the distance to cover: this implies that the velocity profile is not trapezoidal. For this reason a
convergent loop procedure has been developed (shown in the following scheme) that
redefines the values of t1 , vc , and t3 .
29/41
Control algorithm
AN2350
Figure 22. Convergent loop procedure for a trapezoidal velocity profile
Acceleration a
Cruising Speed vc
Max Distance Smax
t1 ≥ t3 / 2
N
t1 = vc / a
t3 = t1 + smax / vc
Y
t1 = t3 / 2 - TR
vc = t 1 . a
t3 = t1 + smax / vc
At the generic sampling time Ti , the covered distance, accordingly to the previous equation,
is equal to si = s (Ti ) ; so the point ( x, y ) of the trajectory to reach in a straight-line motion
at time Ti in the workspace, can be obtained resolving the following system:
⎧si2 = ( x − x1 ) 2 + ( y − y1 ) 2
⎨
⎩ y = mx + c
Knowing ( x, y ) 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.
30/41
Control algorithm
AN2350
Figure 23. The kinematics singularities of the wafer handler during motion
This problem happens only when the manipulator moves along a straight-line passing
through the origin (c = 0) and when the abscissas x1 and x2 are of opposite sign. The
problem has been solved making a change of orientation in the origin in order to cross the
kinematics singularity with an alternative configuration as shown in Figure 24.
Figure 24. How to solve the kinematics singularities during the motion
31/41
Control algorithm
AN2350
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 ( x1 , y1 ) to the origin (0,0) .
●
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 (0,0) to the stop point ( x2 , y2 ) .
Figure 25. Motion path planning scheme to avoid singularities
Start point (x1, y1)
Stop point (x2, y2)
N
Y
c=0
Motion path planning
Straight-line motion
(x1,y1) ⇒ (x2,y2)
Y
sign(x1)
=
sign(x2)
Straight-line motion
(x1,y1) ⇒ (0,0)
Trajectory table as
encoder pulses
Configuration change
180° rotation
Straight-line motion
(0,0) ⇒ (x2, y2)
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.
32/41
Control algorithm
AN2350
To ensure that the point has been reached, a check on the error has been performed in a
such way that if the total error (as sum of the error of the shoulder and of the elbow) is higher
than a fixed value (MAX_ERR_POS=150 ) no new point is passed. The figures below show
the general schemes for the control algorithm ISR and for the scanning of the trajectory
table.
The variables sh_pulses_desired, elb_pulses_desired, total_error, index, flag_error are
global variables.
33/41
Control algorithm
AN2350
Figure 26. PID position control ISR
Reload of Compare unit CC2
and Reset errors
T2
sh_pulses_desired
Encoder feedback for
shoulder and error
computation
error_shoulder
PW0
(max 90% - min 10%)
PID_Position_Control
for the shoulder
Psh , Ish , Dsh
or
(max 65% - min 35%)
T4
elb_pulses_desired
Encoder feedback for
elbow and error
computation
error_elbow
PW1
(max 90% - min 10%)
or
(max 65% - min 35%)
PID_Position_Control
for the elbow
Pelb , Ielb , Delb
Compute total error as
abs(error shoulder)
+
abs(error_elbow)
total_error
>
MAX_ERR_POS
Y
flag_error = 1
34/41
N
flag_error = 0
Control algorithm
AN2350
Figure 27. Trajectory table scanning ISR
Reload of compare unit CC3
if(flag_error)
N
index++;
sh_pulses_desired=shoulder_array[index];
elb_pulses_desired=elbox_array[index];
Y
Stop and Reset Timer T6
Redefine upper and lower PWM limit
(min PWM=10% - maw PWM=90%)
Start Timer T6
Exit
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.
35/41
Control algorithm
AN2350
Figure 28. Values of the absolute encoder for the shoulder
X
7
Homing position
5
6
4
2
0
3
Y
1
Figure 29. Values of the absolute encoder for the elbow
X
7
Homing position
6
5
2
4
3
0
Y
1
Values in the different sectors represent the decimal value of the bits
encoder.
b2b1b0 of the absolute
As we can see, the homing position for the shoulder is characterized by a transition between
the sectors with values 7 and 5, while the elbow by a transition between the sectors with
values 7 and 6.
At the startup of the homing procedure the absolute encoder of the shoulder is read and the
ISR associated to the Capture unit CC16 is enabled. Accordingly to the sector in which the
shoulder is positioned, the shoulder is moved with a fixed PWM. Every time there is a
transition, in the ISR the absolute encoder is read. If the transition is right, then the motor of
the shoulder is stopped (PWM=50%) and the timer T2 of the incremental encoder is reset.
This value is then passed to the control algorithm.
36/41
Control algorithm
AN2350
Now the shoulder is controlled in its home position, the same procedure is executed for the
elbow, using the Capture unit CC22. The control of the shoulder ensures that during the
home procedure for the elbow there's no movement of the shoulder.
The scheme below shows the homing procedure.
Figure 30. The homing procedure
Read Absolute Encoder
of the shoulder and
enable interrupt CC16
while(!home_position_sh)
{
Move shoulder with fixed PWM
}
Read Absolute Encoder
for the shoulder
ISR for the
shoulder
N
This is the right
transition?
Y
Stop the shoulder
Reset incremental encoder
of the shoulder (T2)
Enable interrupt for control
algorithm, and disable
interrupt CC16
home_position_sh=true;
Exit
sh_pulses_desired
Read Absolute Encoder
of the elbow and enable
interrupt CC22
while(!home_position_elb)
{
Move elbow with fixed PWM
}
Control
Algorithm
Read Absolute Encoder
for the elbow
This is the right
N
transition?
Y
Stop the elbow
Reset incremental encoder
of the elbow (T4)
Disable interrupt CC22
home_position_elb=true;
Exit
elb_pulses_desired
37/41
ISR for the
elbow
Control algorithm
4.4
AN2350
Teach and Repeat procedure
Robots are mainly programmed using a technique known as teach and repeat. Using this
technique the control system stores internal joint poses specified by a human operator and
then recalls them in sequence to execute a task. In this way it is possible to store complex
trajectories allowing the robot can execute them with a good reliability and repeatability.
During the teach phase the control of the manipulator is disabled and the human operator
can move the arm as he wants. With a periodic task of 20 ms (associated to the Compare
Unit CC8 and the timer T0) the value of the incremental encoders of the two joints (the
shoulder and the elbow) are stored in the two corresponding arrays: shoulder_array_teach[]
and elbow_array_teach[]. The teach phase duration depends on the size of the two arrays.
Using 3 KByte of the XRAM2 for each array, it is possible to store a trajectory of 30 seconds.
In the repeat phase the couple of points of the two arrays are passed to the control algorithm
as explained before, as well the scanning of the two arrays. The scheme for teach and
repeat procedure is shown in the figure below.
38/41
Control algorithm
AN2350
Figure 31. The teach and repeat procedure
Teach Procedure
Execute homing procedure
Stop the control
Stop the motors (PWM=50%)
ISR for teach
Reload of Compare unit
CC8
Read Incremental
Encoder for the shoulder
(T2)
Reset timer T0
Enable interrupt CC8
index=0;
Start timer T0
Store T2 in
shoulder array teach[]
while index<1500
Read Incremental
Encoder for the elbow
(T4)
Disable interrupt CC8
Stop timer T0
Store T4 in
elbow-array-teach[]
index++
Repeat Procedure
Execute homing procedure
Reset Timer T0
Enable interrupts CC2 and CC3
Start timer T0
39/41
elbow_array_teach[]
Control
shoulder_array_teach[]
Algorithm
Revision history
5
AN2350
Revision history
Table 5.
40/41
Document revision history
Date
Revision
30-Nov-2006
1
Changes
Initial release.
AN2350
Please Read Carefully:
Information in this document is provided solely in connection with ST products. STMicroelectronics NV and its subsidiaries (“ST”) reserve the
right to make changes, corrections, modifications or improvements, to this document, and the products and services described herein at any
time, without notice.
All ST products are sold pursuant to ST’s terms and conditions of sale.
Purchasers are solely responsible for the choice, selection and use of the ST products and services described herein, and ST assumes no
liability whatsoever relating to the choice, selection or use of the ST products and services described herein.
No license, express or implied, by estoppel or otherwise, to any intellectual property rights is granted under this document. If any part of this
document refers to any third party products or services it shall not be deemed a license grant by ST for the use of such third party products
or services, or any intellectual property contained therein or considered as a warranty covering the use in any manner whatsoever of such
third party products or services or any intellectual property contained therein.
UNLESS OTHERWISE SET FORTH IN ST’S TERMS AND CONDITIONS OF SALE ST DISCLAIMS ANY EXPRESS OR IMPLIED
WARRANTY WITH RESPECT TO THE USE AND/OR SALE OF ST PRODUCTS INCLUDING WITHOUT LIMITATION IMPLIED
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE (AND THEIR EQUIVALENTS UNDER THE LAWS
OF ANY JURISDICTION), OR INFRINGEMENT OF ANY PATENT, COPYRIGHT OR OTHER INTELLECTUAL PROPERTY RIGHT.
UNLESS EXPRESSLY APPROVED IN WRITING BY AN AUTHORIZED ST REPRESENTATIVE, ST PRODUCTS ARE NOT
RECOMMENDED, AUTHORIZED OR WARRANTED FOR USE IN MILITARY, AIR CRAFT, SPACE, LIFE SAVING, OR LIFE SUSTAINING
APPLICATIONS, NOR IN PRODUCTS OR SYSTEMS WHERE FAILURE OR MALFUNCTION MAY RESULT IN PERSONAL INJURY,
DEATH, OR SEVERE PROPERTY OR ENVIRONMENTAL DAMAGE. ST PRODUCTS WHICH ARE NOT SPECIFIED AS "AUTOMOTIVE
GRADE" MAY ONLY BE USED IN AUTOMOTIVE APPLICATIONS AT USER’S OWN RISK.
Resale of ST products with provisions different from the statements and/or technical features set forth in this document shall immediately void
any warranty granted by ST for the ST product or service described herein and shall not create or extend in any manner whatsoever, any
liability of ST.
ST and the ST logo are trademarks or registered trademarks of ST in various countries.
Information in this document supersedes and replaces all information previously supplied.
The ST logo is a registered trademark of STMicroelectronics. All other names are the property of their respective owners.
© 2006 STMicroelectronics - All rights reserved
STMicroelectronics group of companies
Australia - Belgium - Brazil - Canada - China - Czech Republic - Finland - France - Germany - Hong Kong - India - Israel - Italy - Japan Malaysia - Malta - Morocco - Singapore - Spain - Sweden - Switzerland - United Kingdom - United States of America
www.st.com
41/41