2 DOF Articulated Pen Plotter

Beng Mechatronics Graduation Project

Gregory Bourke

[Blog under construction]

Synopsis
This project was a 4th year project undertaken to demonstrate the application of scientific and
engineering knowledge gained through the duration of the B(eng) Mechatronics degree at
Nelson Mandela Metropolitan University, Port Elizabeth, South Africa.

While research was undertaken to better understand the concept of simple robotic arm control,
it was noted that a single location with all the information that was needed condensed into a
neat blog or paper was not found. This blog will serve to best convey the concepts used to
control a simple 2 DOF G-Code pen plotter robotic arm and also outline the mechanical design
process, mathematical modeling, electrical design and implementation.

The project involves the design and implementation of an articulated robotic arm with 2 degrees
of freedom (DOF) capable of plotting G-Code commands onto a flat surface with a maximum
size of a standard A3 (420mmx297mm) area. The project consists of a number of stages,
namely conceptualisation, system design, component selection, mathematical modelling of the
arm, coding of the control hardware, implementation of control hardware, performance analysis
and conclusion.
 An Arduino Uno was chosen as the platform used as the control hardware used on which the
control of the arm was implemented, using inverse kinematics.

Performance and test results show that a robotic arm which is capable of plotting commands
streamed to it via the user from a program specifically developed to control the robot arm was
achieved. The robot arm is able to successfully plot circles, lines and font paths generated by a
font path generating program.

A photograph of a font path plotted by streaming GCode commands to the pen plotter
Introduction
This blog serves to compress the final report into a blog which will discuss the final design of the
project and a detailed view on how the system requirements were achieved through
implementation. The blog will skip in-depth comparison between concepts, and move directly
into the final design.

A robotic arm is a mechanical arm which has similar functions to a human arm and in the case
of this project (and usually), is programmable. Robotic arms consist of a series of connected
links of either revolute or prismatic joints which form a kinematic chain. The end of the kinematic
chain, termed the end effecter, is where the tool is located and is analogous to the human hand.
Revolute joints allow rotational motion about an axis, while prismatic joints allow linear
translation along an axis (OSHA, 1999).

Robotic arms are used extensively in industry from a simple robotic arm which performs pick
and place tasks to the most complicated such as a 6 DOF articulated arm capable of a variety
of operations programmed using inverse kinematics, velocity kinematics, trajectory planning and
dynamic modelling. Robotic arms are used because they are versatile and can be
reprogrammed to perform any task necessary within its capability and will faithfully carry out that
task until they are reprogrammed or instructed to stop. Typical robotic arm operations include
pick and place, welding, inspection, assembly and painting. These operations can be achieved
with high speed and precision (Spong, Hutchinson, & Vidyasager, Robot Modeling and Control).

The design and implementation of a robotic arm is an ideal endeavor to demonstrate

mechatronic capability as it is a complete integration of mechanical design, computer science
and electronics. The purpose of this project is to demonstrate the application of mechatronic
ability by applying mechatronic skills and know-how to solve a mechatronic problem.

There have been many designs of 2-DOF robotic arms and a quick search on the internet
proves this. They all vary in construction and size and what their final goals are. Most of the
systems found on the internet, in the case of typical graduating projects, are 2-DOF arms
programmed using inverse kinematics or controlled by external input such as a
potentiometer (Ouyang, 2013), or similar type of master/slave interface. They are built using a
mainly servos and stepper motors for actuation.

Mechanical Design
 The mechanical design of the system includes the arm construction and the reduction gear
couples, platform holding the base of the arm as well as the motors.

Arms
The arms needed to be light weight in order to minimise the inertial load
reflected to the motors through the coupling systems but also strong enough to
hold any other forces in the system without much buckling, bending or large
deflection due to weight. The robot consists of 2 arms, L1 and L2. For
reference, arm L1 is coupled to the shoulder drive motor, while arm L2 is
connected at the elbow, to which the end effector (the pen) is connected.

Mechanical nomenclature

The length of the arms was designed such that the robot could reach the
farthest and closest corner to the robot base.
Arm L1
Design of the arm L1 was influenced by the choice of synchronous belt and
pulley’s (discussed in section 4.1.3 Gear Couples) where the distance from the
shaft holes were set such that they are the same as the centre distance for the
synchronous belt and pulley system. This was to allow the elbow motor to be
placed directly above the shoulder shaft.

Electrical Design
..To Come..
Mathematical Modeling
Inverse Kinematic Equations
Placing the robot base at its position depicted below simplifies in design of the system such that
the arm will only ever be in the “elbow up” position and never in the “elbow down” position. To
solve the angles the arms need to be at in order to achieve a desired location of the end effector
(pen), inverse kinematics is used. Inverse kinematics calculates the required angles of the arms,
theta 1 and 2, in order for the end effector to reach its desired coordinate point.
 The arm in the elbow up position
A simple diagram for the robot arm is given below, where the end effector is represented by the
orange circle. (x0,y0) is the coordinate frame for arm l1, and (x1,y1) the coordinate frame for
arm l2.

Diagram of system

noting that

and using the law of cosines

Substituting a for h, b for l2, c for l1 and A for (pi - theta2) and solving for theta2 yields
to find theta p, atan of the new coordinate was calculated

to solve for alpha, the law of cosine was again used. Substituting all the relevant variables and
solving for alpha yields

noting that theta 1 is simply the addition of theta p and alpha, theta 1 is calculated by

Thus, the values for theta 1 and theta 2 which are required for the end effector to be at a
desired location can be found. These equations are later implemented in the Arduino code
along with other equations (discussed later under the main/initialise code segment ) to achieve
the desired number of steps for the stepper motors in order to reach a desired location.

Dynamic Equations of Motion

With the use of stepper motors, it was imperative to calculate the maximum acceleration the
steppers could handle according to their available torque Vs. the load they were driving.

Calculation of the torque required to accelerate the arms is not simply a case of applying the
torque equation , but is rather a dynamic problem requiring a dynamic solution. Energy
methods utilising the Lagrangian was used for this purpose.

The following equations describe the torque at each joint, calculated using the Lagrangian of the
system. The derivation is mathematically intensive, and thus the derivation is shortened as
much as possible.
The lagrangian is calculated as the difference between kinetic and potential energies of the
system

Thus the kinetic and potential energy of the system need to be computed. The velocity of the
center of arm L2 is found by differentiating its position

Where lm2 is the distance from the axis of rotation to the center of mass for arm L2. The total
velocity of the center is then

where

The following identities are used to simplify the addition

and
which yields

The rotational kinetic energy of arm is calculated using

It is important to note that IA is the inertia of arm around the axis of rotation of arm L1.

The kinetic energy of arm L2 is the summation of its rotation energy due to its own rotation, and
the rotation and linear velocity imparted on the arm from the rotation of arm L1 and is calculated
using

Where ID is the moment of inertia of the arm at its mass centre, or point D. The total
kinetic energy of the system is then

Multiplying out the brackets and simplifying by factorizing out common time differentials of both
angles yields

The equation for potential energy is given as

Since the arms are in the horizontal plane and gravity will not cause the arm to move, the
potential energy of the system is always zero. Thus, the Lagrangian is
Thus the equations of motion are given as

Which yields

It is clear from these equations, that the torque is dependent on the acceleration/deceleration as
well as the mass, lengths of certain mass moment points away from other points and inertia of
the arms. The construction and assembly of the parts on the arms make it near impossible to
calculate the moments of inertia analytically. To approximate the inertia's IA and ID , AutoCAD
inventor was used to model the arms by assigning materials to known components, such as the
aluminium arms and shafts, and by manually inputting the weight of components which are
constructed of a variety of materials. This is an approximation since Inventor will assume
uniform mass distribution of parts such as the 9g servo, which is not entirely correct. But as
stated, it served as an approximation. The mass of each arm was also read from Inventor, but
the arm configurations were also weighed to double check. The results from physical weighing
and approximation using inventor coincided.

The following values were given from Inventor:

Substituting these values into the equations for the torques at each joint yields
The Stepper motors are set to accelerate at a rate of 500 pulses per second per second. Due to
the microstep ratio, the motors step at 0.255 degrees per pulse. The accelerations are also
reduced by ratio of the gears

Noting that these torques are the torques to accelerate the inertial components of the arms only,
and not the inertia of the motors rotor, the torque required from each motor to accelerate the
inertial load and the inertia of its rotor is then the summation of the torque required to accelerate
its own rotors inertial load and the torque required to accelerate the arms reduced by the gear
ratios, as follows
 The result shows that the torque to accelerate each arm is extremely low, and this is attributed
to the fact that the motors need only to move the arms in the horizontal plane. If the arms were
to be moved in the vertical plane, the Lagrangian would include potential energy, and the
motors would have to apply torque to rotate the weight of the arms, and not just the inertia of
the arms.

IT Design
There are two parts of software for the robot arm - the program which is loaded onto the
controller platform (Arduino) as well as the software used to communicate with the robot arm
from a computer. A Microsoft Windows executable program was designed to stream G-Code
commands via serial communication to the Arduino and is called the robot control program.

Setting the motors to maximum speed from rest could result in the motor being unable to move
the load and therefore skipping steps, or cause damage to the plastic gear used in the shoulder
coupling system which are both extremely undesirable. To curb this, an acceleration profile for
the stepper motors was needed and was implemented by the addition of the AccelStepper
Arduino library which is based on calculations derived by David Austin (Austin, 2005). The
AccelStepper library improves on the standard stepper library in the following ways (McCauley):
 Acceleration and Deceleration support
 Supports independent concurrent stepping on multiple simultaneous steppers
 Non-blocking API functions
 Support of 4 wire steppers
 Slow speeds are supported
 Compatible with the EasyDriver
Thus the AccelStepper library will be used to control the stepper motors.

Arduino Software
The software implemented on the Arduino consists of the following code segments:
 Main/Initialise
 Loop
 To Point
 Park
 G-Code Implementation
 Circle
Main/Initialise
The main/initialize segment contains all necessary functions and is where the
setting up of the I/O’s, global variables, defining of steppers, servos and
inclusion of libraries occur. Two libraries are included into the sketch, namely
the AccelStepper and Servo library.

Global variables are initialised to store the current angles of each arm, as well
as the current x and y position of the arm respect to the robot frame. The home
position can either be set at the top left hand corner or in the middle of the
canvas, using the robot control software (see 4.3.2 Robot Arm Control
Program). For example, Home is defined at the middle of the canvas with the
coordinates (210,149). This translates to the following set of values which were
used to define the current x, y, theta 1 and 2 values

 CurrX = 265 (CurrX with respect to robot frame, 210 canvas frame)
 CurrY = 137 (with respect to robot frame, 149 canvas frame)
 theta1= 84.67 (CurrAngle1, measured positive, anticlockwise)
 theta2= 113.55 (CurrAngle2, measured positive clockwise)
A SPD (Steps per Degree) variable is defined for each motor in order to determine the number
of steps each motor is required to move per degree. The SPD for each motor is dependent on
the step angle, gear ratio and microstepping ratio of the drivers for the motor according to the
formula

Where np is the number of teeth on the pinion gear, nd is the number of teeth on the driven
gear and a microstep ratio of 8 microsteps per full step. SPD for the shoulder is calculated by
substituting 12 and 145 for np and nd respectively

For the elbow SPD, substituting 20 and 80 for np and nd respectively yields

The arm lengths are also defined in the main class, as well as their squares to decrease the
computational time required within the inverse kinematics calculations.

Loop
The loop function runs continuously checking the Estop, as well as checking if data is available
on the serial port sent from the robot control program. After a command is performed, the
Arduino will jump back to this loop, ready for the next command from the robot control program.

Once the port is open, the Arduino constantly scans the serial input buffer for available data. If
data is available, a while loop reads each available character into a string (content) of length 15.
This ensures the entire G-Code command is received. An if statement is implemented inside the
continuous loop straight after the serial read while loop to check whether there is information
inside the content string. If the content string is not a null string, the content is passed through a
series of if statements, checking which code have been sent through from the Robot control
software. The following are the codes which the PC software sends to the Arduino via the serial
link:

 “JU” - The robot controller software has requested that the robot be jogged, up. This
correlates to an increase in the CurrY position by 1.
 “JD" - Jog the robot, down – correlates to a decrease in CurrY position by 1
 “JL” - Jog the robot, left – correlates to a decrease in CurrX position by 1
 “JR” - Jog the robot, Right – correlates to an increase in CurrX position by 1
 “HO” - Set current position of the robot arm as the home position situated at (0,297)
 “H1” - Set current position of the robot arm as the home position situated at
(210,297149)
 “GH” - Send the robot to defined home position.
To Point
The to point (called toPoint) function translates the required canvas coordinate to the robot
frame coordinate and then calls the function (set_arm) where the mathematical calculations
based on the inverse kinematics of the system are performed (See code attached as
Addendum). The function receives two parameters, x and y, which are the canvas coordinates
of the new point to which the arm is required to move.

The set_arm algorithm calculates the absolute difference between the current angle (stored in
Currθi, where i = {1.2}) of each motor, which is the angle to which each motor needs to move in
order to reach the desired coordinates from the current coordinates. This value is then
multiplied by the SPD of each motor to determine the number of steps required per motor to
move the pen from the current location to the new location, after which the current angle for
each arm, current x and current y variables are updated.

The motor directions are decided by comparing the angles, for the shoulder motor; if the new
angle is greater than the previous angle, the motor needs to rotate clockwise and anti-clockwise
if the new angle is smaller than the previous angle. The elbow motor is opposite; an increase in
angle requires anticlockwise rotation, while a decrease requires clockwise rotation of the motor.
This communicated to the motor by sending negative amount of steps to the motor for clockwise
rotation, and a positive number of steps for anti-clockwise rotation.

Since stepper motors are open loop control, there is no feedback from the stepper motors to
determine whether the stepper motor has reached its destination or not. Therefore, some sort of
method needs to be implemented in the software to allow the motors to reach their destination
before the new set of commands is sent through. This method only considers the theoretical
number of steps the motor still needs to move according to the theoretical number of steps it
needed to perform, and as such, will not know if the stepper motor has physically missed a step.

A common method to allow a stepper motor to reach its destination is to simply create a delay
of approximately 10ms into the function passing through the number of steps the motor is
required to move, which gives a fixed time for the motor to complete its steps. If a new target
number of steps is sent through after the delay, before the stepper motor has completed its
previous step count, the motor will technically discard the previous unfinished steps, and
perform the next step count. The delay() method is a crude, and as such was not used as it
introduces jerky motion into the movement of the arm if motions are completed a considerable
time before the delay was completed, as well as not being completely certain whether the motor
had in fact reached its destination before receiving the new set of coordinates.

Park
Parking of the arm is achieved by simply moving the motors back to the home position, as
defined by the user.

G-Code Implementation
G-Code (G programming language) is the most widely used numerical control programming
language used in many implementations. G-Code is a simple language which instructs
computerised machinery (the robot arm) on how to move. These commands include how fast to
move, where to move to and what kind of movement it should be (G-code, 2013).

The robotic arm movement can either be rapid movement or linear interpolation. Rapid
movement performs only a single inverse kinematic calculation of the joint angles required for
the arm to reach the destination coordinate. In rapid movement, there is no certainty of the path
taken. Performing the calculation once will allow the motors to accelerate to their maximum
speeds.

Linear interpolation divides the movement into smaller steps, calculating the inverse kinematic
joint angles at small intervals between start and end coordinate using linear interpolation to
calculate the next sub coordinate. Since the steps are small, the arm will never fully accelerate
to its maximum speed, and thus is much slower than rapid movement. The idea is shown in the
image below where black dots indicate start/end points and blue dots indicate intermediate
points where the joint angle calculations are performed.

Rapid movement (Left) Vs Linear interpolation (Right)

 Table of G-Code Commands understood by the controller

The G-Code commands are sent to the robot controller (Arduino) through an open serial link
between a computer and the Arduino. A robot arm control program was created to perform this
task, and is discussed later. To illustrate the use of G-Code commands, the G-Code commands
in the provided screenshot of the robot control software are explained:

 G00 Z0
 G00 - Rapid positioning
 Z0 – Lift pen up off canvas
 G00 X60 Y60
 Rapid movement to starting position of the line to be drawn
 G00 - Rapid positioning
 X60 – Next X position (rapid positioning)
 Y60 – Next Y position (rapid positioning)
 G01 Z-1 F0.0
 G01 – Linear Interpolation
 Z-1 – Put down pen onto canvas
 F0.0 – Feed rate (for milling, ignored)
 X60 Y80
 X60 – next X position (linear Interpolation)
 Y80 – Next Y position (linear Interpolation)
 X60 Y81
 X60 – next X position (linear Interpolation)
 Y81– Next Y position (linear Interpolation)

The feed rate (F) command is not used by the robot arm since the robot has been given a set
speed and acceleration profile, and thus is ignored by the G-Code parser.

The G-Code parser will execute the next command according to the last G-Code command
received. For the drawing of lines and other continuous complex shapes, the program needs
only to send through the linear Interpolation command G01 once – any coordinate received
following the command will be executed using the last received command which is G01, Linear
interpolation.

Circle
Plotting a circle is achieved by plotting the top and bottom arc half separately. In Figure 69, the
nomenclature for a circle is given.

Circle plotting nomenclature

The radius and centre coordinate (xC, yC) are required. The Starting point of the circle is then
calculated by subtracting the radius from the centre X coordinate. To plot the top half, an x
variable is incrementally increased by 1 from the starting x value to the finishing x value. After
each increment, the corresponding y coordinate is calculated using
Each incremental x and y coordinate is then sent do the toPoint command, which calculates the
required steps of each stepper motor to achieve the new desired coordinate.

Plotting the bottom half then follows and is plotted in the same manner as the top half, but with
the x variable being incrementally decreased.

The code to plot a circle

Linear Interpolation
Linear interpolation divides the linear distance from the current coordinate to the next coordinate
into incremental steps. The robot has been programmed such that all incremental steps are
one millimeter.

When plotting, there are cases with regards to the relationship between the new coordinate and
the previous coordinate, which are:

 The new x coordinate is larger than the previous x coordinate

Example: (100,100) → (150,130)

The program incrementally increases x by 1 each step until x is equal to the required position.
The new y coordinate for each x increment is calculated using the gradient between the
previous and new coordinate. The coordinate at each incremental step is sent through to the
set_arm function, which calculates the arm angles for the new incremental position and moves
the stepper motors accordingly.

 The new x coordinate is smaller than the previous x coordinate

Example: (150,100) → (100,130)
The program incrementally decreases x by 1 each step until x is equal to the required position.
The new y coordinate for each x increment is calculated using the gradient between the
previous and new coordinate. The coordinate at each incremental step is sent through to the
set_arm function, which calculates the arm angles for the new incremental position and moves
the stepper motors accordingly.

  The new x coordinate is the same as the previous x coordinate

 The new y coordinate is larger than the previous y coordinate
Example: (100,100) → (100,130)
The program incrementally increases y by 1 each step until y is equal to the required position.
The new y coordinate and the unchanged x coordinate are sent through to the set_arm function
each increment.

 The new y coordinate is smaller than the previous y coordinate

Example: (100,100) → (100, 70)
The program incrementally decreases y by 1 each step until y is equal to the required position.
The new y coordinate and the unchanged x coordinate are sent through to the set_arm function
each increment.

Fonts
The implementation of G-Code enabled fonts to be drawn. Font path G-Code commands can be
generated using DeskEngrave (www.deskam.com), which is a free desktop application enabling
the user to generate and export the G-Code commands for a font path, to text file. This list is
then loaded into the Robotic arm control software.

DeskEngrave accepts text from the user, as well as the origin, font size, font style and either
width or height of the font – If the width is chosen as the fixed variable, the program calculates
the associated height and vice-versa.

For example, the word “Test” is entered into the text box with the “Cooper” font, origin of (50,
50), and width of 200. The coordinates will be in millimeters since the robot is programmed in
millimetres. Note how the Height is 0, since this will be calculated by the program. It is advisable
to choose the width of the word as the user entered variable, since the length is limited to the
width of the canvas (i.e. should not be larger than 420)

Test text in DeskEngrave

 Output from robot arm

System Cost

The system cost came to a total of R 2,108.42. The material used in the construction of the
arms were machined from scrap lengths of aluminium rectangular bar equal to the design
parameters, and the shafts machined from lengths of scrap aluminium. The 5mm bearings were
sourced from old weighing scales, and as such were also not bought. Thus, an estimate was
added for if they were to be bought if possible, and their design parameters calculated
accordingly.

Machining labour was also free, as all the machining was done privately.
 Cost of system components

Posted by Gregory Bourke at 09:22

Email ThisBlogThis!Share to TwitterShare to FacebookShare to Pinterest