Design and Control of a Pantograph Robot

Overview

The goal of this project was to design and build a 2-DOF Pantograph Robot. The user would be able to specify and X and Y location for the end point of the pantograph, along with different paths that the robot arm could take. The basic circuit design and computer code was taken from the High Speed Motor Control project and adapted to work with our robot design.

Kinematics

The kinematics for our robot design were derived from The Pantograph Mk. II - A Haptic Instrument. The forward and inverse kinematics as used in the MATLAB program are shown below, along with diagrams from the paper showing how the reference frames are defined in our setup. These angles are all calculated in radians, other parts of the programs convert this to degrees for graphing and user interfaces. The full programs can be found in the zip files below.

Forward Kinematics

x2 = L1 * cos(theta);
y2 = L1 * sin(theta);

y4 = L4 * sin(alpha);
x4 = L4 * cos(alpha) - L5;

two_to_four = sqrt((x2-x4).^2 + (y2-y4).^2);

two_to_h = (L2^2 - L3^2 + two_to_four.^2) ./ (2 * two_to_four);

yh = y2 + (two_to_h./two_to_four).*(y4-y2);
xh = x2 + (two_to_h./two_to_four).*(x4-x2);

three_to_h = sqrt(L2^2 - two_to_h.^2);

x3 = xh + (three_to_h./two_to_four) .* (y4-y2);
y3 = yh - (three_to_h./two_to_four) .* (x4-x2);

Inverse Kinematics

L13 = sqrt(x3.^2+y3.^2);
L53 = sqrt((x3+L5).^2 + y3.^2);

alphaOne = acos((L1.^2 + L13.^2 - L2.^2)./(2.*L1.*L13));
betaOne = atan2(y3,-x3);
thetaOne = pi - alphaOne - betaOne;

alphaFive = atan2(y3,x3+L5);
betaFive = acos((L53.^2 + L4.^2 - L3.^2)./(2.*L53.*L4));
thetaFive = alphaFive + betaFive;

Circuit Design

Robot Circuit Schematic

The complete circuit schematic for the current robot setup is displayed to the right. A brief description of the components in the circuit and what they do follows.

This is essentially one smaller circuit repeated for the second motor. Each motor is powered by an L298N H-bridge amplifier which is powered by 12 volts currently. These amplifiers are capable of sending out four different signals, however in this application the matching inputs and outputs are connected in parallel in order to increase the available current.

The PIC sends three signals to each H-bridge, one for each motor lead and an enable signal that activates the amplifier. These signals pass through 4N27 optoisolators to shield the motors from signal noise.

Each motor encoder is connected to an LS7083 encoder counter chip to allow the PIC to keep track of the motor angle in order to control the position of the end effector. This connection is made through a DB9 connector that interfaces with the plug attached to each motor.

There are two hall effect sensors that are mounted on the robot base and connected to the PIC. These allow the user to set the position of the arms by manually moving them at the beginning of the program until they are tripped by small magnets mounted on the robot arms.

The PC communicates with the PIC via RS232 using a MATLAB program. This allows the robot arm to be completely controlled by a PC once the program is loaded onto the PIC chip.

Parts List

Electrical Components Needed.	Quantity	Data Sheets
PIC 32 on NU32 board	1	Introduction to the PIC32
H-bridges L298	2	data sheet
Optoisolators 4N27	6	data sheet
Quadrature Up/Down decoders LS7083	2	data sheet
Hall Effect Sensors	2	Hall Effect Sensor
35V 9A Schottkey Diodes 90SQ035-ND	8	data sheet
Harmonic Drive RH-8D-3006 Actuator with 100:1 gear ratio and 1000 count per revolution encoder	2	data sheet
Power Supply	1	N/A

Operating Instructions

The steps to operate the robot arm in its current configuration are as follows:

1. Plug in and check all connections. Connect the power USB cord for the PIC and the RS-232 cable to the laptop that will be running the MATLAB program. Plug in the power supply for the motors. Check to make sure that the motor leads are connected to the H-bridge amplifiers and the DB9 encoder cables are plugged in.

2. Run the MATLAB program. Open MATLAB and run the me333gui.m file contained in the folder located in the MATLAB Code section of this page. This will open the GUI interface that sends commands to the robot arm. You may have to choose 'Add to path' if a window pops up telling you that the folder containing the M-file is not in the predefined path.

3. Connect to the COM port. There is a box in the GUI that says "COM#". Replace the # sign with the number of the serial port that the RS-232 cable is connected to and press connect. If you select an unavailable port an error message will appear in the main MATLAB command window. It will also list the available ports, the RS-232 cable should be one of those. If you accidentally choose the wrong port out of the available ones the MATLAB program will work but the motors will do nothing. Simply press disconnect and try a different port.

4. Home the arms. Turn on the PIC using the switch on the NU_32 development board. Before the robot can do anything the arms must be homed to determine what angle they are currently at. Move the arm attached to motor 2 by hand until it trips the hall sensor at approximately 180 degrees. LED 0 on the board will then turn off. Then move the other arm to roughly 0 degrees until the other hall sensor is triggered, which will turn off LED 1. The robot is now ready to run.

5. Choose a path and run it. There are currently three available options for controlling the robot arm. The 'Go To' option moves the end point of the arm to a user-defined position in the x-y plane. The duration of this move is set in the code on the PIC and can be easily adjusted, though not in the MATLAB interface. The 'Circle' option allows the user to define an ellipse by choosing the center point, radius in both X and Y directions, and length of time in seconds that the arm takes to trace it out. The 'Trace' option allows the user to define a path consisting of four points that the arm will trace out. In order to run any of these options, click on the radio button, type in your desired coordinates, and click Start.

6. Logging Data. In order to record and graph the encoder data, the motor must be running a Circle or Trace path, and the Loop Path option must be checked when the path command is sent. This tells the robot to repeat the circle or path until it receives another command. Once the arm has completed at least one cycle you can receive the data by pressing the Get Data button. Alternately, you can check the Continuous Update box which will continually ask the PIC for data and graph it. You can choose a graph of motor angles vs. time or x position vs. y position by choosing that selection before asking the PIC for data. Each graph displays the desired data and actual recorded data for each motor, allowing the users to see if any adjustments are needed in the gain values.

7. Adjusting the gains The robot arm is currently controlled by PID control based on the gains entered in the corresponding boxes in the GUI. The program will automatically send out these gains whenever a Circle or Trace command is sent, or you can press the Update button to send these new values to the PIC at any time.

Notes:

• In order to stop the arm in its current position the user can press Pause. A new path can then be chosen. Alternately, a new path can be sent while the PIC is running a different path and it should switch to the new path on the fly.
• If the arm stops suddenly it has most likely reach the predefined limits set in the PIC code to prevent the motors from damaging the arms. If this is the case the red warning LED on the breadboard will be lit. Simply move the arms by hand to a safe position and choose a different point or path.
• Occasionally the Get Data function will return an error. This is a recent phenomenon and the immediate cause is unknown. It appears that sometimes MATLAB does not completely convert the string data to integers, which prevents the program from calculating the angles. Simply press the Get Data button again and it should work properly. This problem is fairly rare.