computer controlled robot arm - uvic department of electrical and

University of VictoriaFaculty of Engineering

Summer 2004 ELEC499A

Computer ControlledRobot Arm

http://web.uvic.ca/~vickayak/web/

Supervisor:Dr. Subhasis Nandi

Group Members:Mehran Haji Rasouliha

Dylan SprouleJason Wong

ii

Table of Contents

LIST OF TABLES..................................................................................................................................................... iii

LIST OF FIGURES................................................................................................................................................... iii

1 PROJECT OVERVIEW................................................................................................................................... 1

1.1 INTRODUCTION................................................................................................................................................ 11.2 CRS-PLUS ROBOTIC SYSTEM.......................................................................................................................... 11.3 PROJECT GOAL................................................................................................................................................ 11.4 DIVISION OF TASKS ......................................................................................................................................... 21.5 TASK COMPLETION TIMETABLE ...................................................................................................................... 21.6 PROJECT PART SOURCES ................................................................................................................................. 3

2 ROBOTIC ARM ............................................................................................................................................... 4

2.1 INTRODUCTION................................................................................................................................................ 42.2 TYPES OF ROBOTIC ARMS ............................................................................................................................... 42.3 PROJECT ROBOT ARM ..................................................................................................................................... 5

3 MECHANICS.................................................................................................................................................... 6

3.1 INTRODUCTION................................................................................................................................................ 63.2 TYPES OF ELECTRIC MOTORS.......................................................................................................................... 63.3 MOTORS USED IN PROJECT ............................................................................................................................. 8

4 ELECTRONICS.............................................................................................................................................. 10

4.1 INTRODUCTION.............................................................................................................................................. 104.2 STEPPER MOTOR DRIVING CIRCUIT .............................................................................................................. 104.3 INTERFACING ELECTRONICS WITH COMPUTER PARALLEL PORT ................................................................... 114.4 BUILDING CIRCUIT ........................................................................................................................................ 12

5 SOFTWARE.................................................................................................................................................... 14

5.1 INTRODUCTION.............................................................................................................................................. 145.2 SOFTWARE FEATURES ................................................................................................................................... 145.3 FUNCTION DESCRIPTIONS.............................................................................................................................. 15

6 DISCUSSION .................................................................................................................................................. 17

7 REFERENCES................................................................................................................................................ 18

Appendix A – Schematic Diagram

Appendix B – Source Code

iii

List of Figures

Figure 1. Three square wooden blocks moved from stacked formation into pyramid formation. . 1Figure 2. The Armatron toy robot arm made by Radio Shack in the 1980’s................................. 5Figure 3. The original DC motor used to drive all gearing mechanisms........................................ 6Figure 4. 5 wire unipolar stepper motor and movement of shaft.................................................... 7Figure 5. Fittings made for each stepper motor out of PVC rod with gear attached to end. .......... 8Figure 6. Stepper motors with gear fittings placed onto chassis of robot arm................................ 9Figure 7. Driving circuit for one stepper motor. ........................................................................... 11Figure 8. A black box representation of the inputs and outputs of the electronics....................... 11Figure 9. Robot arm circuitry built onto a prototype board. ......................................................... 12Figure 10. LM317 voltage regulator providing a regulated 5V to the circuit............................... 13Figure 11. Screenshot of robotic arm control software written in Visual C++............................ 14Figure 12: Robotic arm software flowchart. ................................................................................. 15Figure 13. Computer controlled robot arm moving blocks in pyramid formation. ...................... 17

List of Tables

Table 1. Project tasks and group members task is assigned to. ...................................................... 2Table 2. Completion date of project tasks. ..................................................................................... 2Table 3. Stepper motor number and part of robot arm that it controls. .......................................... 8Table 4. Description of parallel port connections and purpose. ................................................... 10

1

1 Project Overview

1.1 Introduction

The objective of our ELEC 499 project is to design and build a computer controlled robot armwith similar functionality to the commercially available CRS-Plus robot arm. The teammembers are Mehran Haji Rasouliha, Dylan Sproule, and Jason Wong.

1.2 CRS-Plus Robotic System

The CRS-Plus robotic system consists of a CRS-Plus mechanical manipulator with a standardhand, a system controller, and a teach pendant. The mechanical manipulator uses servomotors tomove in five degrees of freedom. The teach pendant is used to move the robot arm manuallyjoint by joint. The CRS-Plus is initially taught manually with the teach pendant. Once all thepoints are saved the system controller can move the arm and repeat the motions with a highdegree of accuracy and precision.

1.3 Project Goal

Unfortunately, the CRS-Plus robotic system costs thousands of dollars to purchase. The goal ofthis project is to create a computer controlled robot arm with similar functionality which costsmuch less. The scope of the project includes the designing and building of the hardware andsoftware for a comparable robot arm. The completed project will consist of a robot arm with fivedegrees of freedom, computer controlled electronics, and software to teach and control the arm.Successful completion of the project will be achieved when the project’s robot arm can performthe same task as required of the CRS-Plus robot arm in ELEC 360’s Laboratory #4. The task isto move three square wooden blocks stacked on top of each other into a pyramid formation asshown in Figure 1.

Figure 1. Three square wooden blocks moved from stacked formation into pyramid formation.

2

1.4 Division of Tasks

The project tasks were defined and divided amongst the group members as shown in Table 1.

Table 1. Project tasks and group members task is assigned to.Task Details ResponsibilityMechanics ? Removing of levers and gears

? Creating fittings for stepper motors? Implementing motors into arm

chassis

Mehran, Jason

Electronics ? Designing control electronics forstepper motors

? Prototyping electronics and buildingcircuit board

Jason, Dylan

Software ? Accessing parallel port? Creating control signals to drive

stepper motors

Dylan, Jason

Report ? Documentation of project? Schematics and flowcharts

Jason, Dylan, Mehran

Webpage ? Design and creation of reportcontents into a web site

Dylan, Jason

1.5 Task Completion Timetable

The project tasks were completed on or before the dates shown in Table 2.

Table 2. Completion date of project tasks.Task Completion DateProject definition May 21, 2004Hardware design May 21, 2004Hardware creation June 4, 2004Electronics design June 11, 2004Electronics creation June 18, 2004Software design June 25, 2004Software creation July 9, 2004Connecting hardware and software components July 16, 2004Testing and calibration July 20, 2004Project completion and poster presentation July 23, 2004Final report and webpage submitted July 30, 2004

3

1.6 Project Part Sources

The project parts are listed in Table 3.

Table 3. Parts acquired for completion of project.Qty Description Part# Price Source

6 IC 4bit bi-direction shift register 16-DIP CD40194BE 1.29 Digi-Key4 IC driver darlington 7ch 50V .5A 16DIP ULN2003AP 0.63 Digi-Key1 12V zener diode 1W 5% DO-41 1N4742A-T 0.5 Digi-Key2 Quad 2-input NOR gate 14-DIP CD4001UBE 0.7 Digi-Key1 Dual 1:4 decoder/demultiplexer 74F139PC 0.35 Digi-Key1 Radio Shack Armatron Robotic Arm --- 6.99 Value Village6 12V Stepper motor, 1.8 deg/step --- 0.00 5 ¼” floppy drives

2 feet 1” diameter solid PVC rod --- 20.00 Industrial Plasticand Paints

4

2 Robotic Arm

2.1 Introduction

There are many different types of robotic arms. The basic parts of a robot arm are the arm,forearm, body and end effect manipulator. The four main types of robot arms are revolute, polar,cylindrical and Cartesian coordinate. The degrees of freedom or the number of axis classify thetype of robot. The area that a robot covers is called the work envelope.

2.2 Types of Robotic Arms

There are many different types of robotic arms. The joints and movements of each arm create adifferent work envelope. The number of axis is directly related to the maneuverability of arobotic arm.

Revolute Coordinate Robot ArmThe revolute coordinate robot arm is very similar to the human arm and it is capable of many ofthe same motions as a human arm. But the design of the this kind of robot is a little bit differentfrom human arm due to the complexity of the human shoulder joint. The shoulder of theRevolute robot rotates by spinning the arm at its base. The movement of the shoulder is done byflexing the upper arm member back and forth while the elbow joint moves the forearm up anddown. This kind of robot is very flexible and looks somewhat like a human arm.

Polar Coordinate Robot ArmThe polar coordinate robot arm is very flexible and can grasp different kinds of objects aroundthe robot. The robot rotates by a turntable base and the elbow joint is the second degree offreedom and moves the forearm up and down. This robot achieves the third degree of freedom bychanging the reach of the forearm. The inner forearm has the job of bringing the gripper close oraway from the robot.

Cylindrical Coordinate Robot ArmThe cylindrical coordinate robot arm has the shape of a robotic forklift. The area which this armworks in is the shape of a thick cylinder. The rotation of the shoulder is done by revolving thebase like the polar coordinate system. The forearm of this robot can grasp objects of differentheights by moving the forearm up and down the column. The forearm also has a threedimensional work envelope. Cartesian Coordinate Robot ArmThe Cartesian coordinate robot arm consists of a carrier belt like track that makes the arm goback and forth. The work envelope of this robot arm is shaped like a box. The forearm of therobot moves up and down along the column and contains an inner arm that can reach both closeand far.

5

2.3 Project Robot Arm

The project’s robot arm will originate from an Armatron toy robot arm shown in Figure 2. It wasmade by Radio Shack and Tandy in the 1980’s. The robot arm was acquired from the secondhand sore Value Village for $6.99. The arm is most similar to the revolute coordinate robot arm.It has five degrees of freedom and has a gripper as the end effect manipulator. The arm isoriginally controlled by moving the two joysticks which in turn engage a DC motor to differentgearing mechanisms. It was decided to use an already fabricated arm because it made the projectless time consuming and cheaper. The ultimate goal of the project is more control oriented thanmanufacturing oriented.

Figure 2. The Armatron toy robot arm made by Radio Shack in the 1980’s

6

3 Mechanics

3.1 Introduction

The toy robot arm was originally controlled by moving the two joysticks. The two joystickswould in turn engage a DC motor to drive different gearing mechanisms as show in Figure 3.Motors in robots are like muscles in the human body. There are a large variety of motorsavailable but only certain types of motors are useful in robotic arms.

Figure 3. The original DC motor used to drive all gearing mechanisms.

3.2 Types of Electric Motors

AC motorsAC motors are commonly found in industrial factories driving conveyor belts or other heavymachinery. There are very few robot arms that use AC motors. Some of the reasons are due topoor accuracy, low starting torque and poor dynamic response.

7

Continuous DC motorsDC motors are more commonly found in robot arms. Continuous motors move as long as poweris applied and stops as soon as the power is removed. For continuous rotation the power has tobe pulsed to the motor and this is done by a computer or indexer. Different types of continuousmotors include brushless, permanent magnet and variable reluctance. However, these motors donot provide position control and are not very useful for precise robotic arm movement.

Servo motorsA servo motor is a DC motor combined with some position sensing parts. Typically servomotors have 3 wires coming out from the motor. Two lines are for power and the third line is acontrol input. A pulse width signal applied to this input tells the motor to what position it shouldbe moved to. The inside of a servo motor consists of a DC motor, a geartrain, limits stops and apotentiometer for position feedback. Robot arms using servos monitor the robot’s axes and itscomponents for position and velocity.

Stepper motorsA stepper motor is very simple DC motor because it has no brushes or contacts. It operates byhaving its magnetic field electronically switched to rotate the armature magnet around. Thissetup allows the motor to rotate a specific angle and stop. There are two types of stepper motorswhich are bipolar and unipolar types. The bipolar stepper motor consists of two coils and thecurrent direction is reversed in each coil to achieve four separate positions. The other type ofstepper motor is a unipolar type and is shown in Figure 4. The unipolar stepper motor consists offour coils. When each coil is energized individually and in proper sequence the motor shaft turnsthe specified number of degrees per step.

Figure 4. 5 wire unipolar stepper motor and movement of shaft.

8

3.3 Motors Used in Project

Stepper motors were chosen for this project because of their easy circuit implementation andavailability. The stepper motors for this project were taken from old 5 ¼” computer floppydrives. The read/write heads used stepper motors to move back and forth along the magneticdisk. The stepper motors did not cost any money as they were salvaged from old computers andjunk piles.To allow the robot arm to be computer controlled the DC motor was removed and replaced withsix stepper motors. The system controller software being run on a computer will control andregulate the number of steps of each motor. The motors were all placed into the base of the robotarm because that is where all the mechanisms and driving gears were. The steppers motors wereput directly in line with the gears in the base with the aid of some fittings. The fittings weremade for each stepper motor using a 1” solid PVC rod as shown in Figure 5. At the end of eachrod a ¾” hole was drilled to hold the shaft of the motor. A pilot hole was made at the other endof the rod where a gear was secured using screws.

Figure 5. Fittings made for each stepper motor out of PVC rod with gear attached to end.

All six stepper motors and robot arm were mounted onto a wooden platform as shown in Figure6. The motor number and its function is described in Table 3.

Table 3. Stepper motor number and part of robot arm that it controls.Motor Description

1 Gripper2 Wrist up/down3 Main arm tilt up/down4 Middle arm left/right5 Wrist rotation (360 degrees)6 Main arm rotation (360 degrees)

9

Figure 6. Stepper motors with gear fittings placed onto chassis of robot arm.

10

4 Electronics

4.1 Introduction

The stepper motors are controlled through the computer’s parallel port. A separate controlcircuit was created to minimize the number of control lines required of the computer and also toprovide 12V power to each separate coil of the motor. A description of each input to theelectronics circuit and connections to the parallel are summarized in Table 4.

Table 4. Description of parallel port connections and purpose.Name Description Parallel Port Pin

Generic Clock Squarewave generated by computer,frequency controls speed of motor

1 (C0 - Inverted)

Motor 1 Clock Drives Motor 1 14 (C1 - Inverted)Motor 2 Clock Drives Motor 2 17 (C3 - Inverted)ResetNot Initialize shift registers with default value 16 (C2)Motor Select Selects which motor receives clock input 3,2 (D1,D0)Motor X Direction Controls direction of motor 9,8,7,6,5,4 (D7-D2)

Motor 6 = D7, …,Motor 1 = D2

4.2 Stepper Motor Driving Circuit

The stepper motors used in this project are 5 wire unipolar motors. Each stepper motor has itsown driving circuit consisting of a 4 bit shift register and four darlington sink drivers as shown inFigure 7. A total of 6 circuits as shown in Figure 7 were produced. To drive each stepper motorthe shift register must first be loaded with an initial bit pattern by setting direction high andResetNot low. After initialization a clock signal is fed into the shift register and a direction ischosen. To reverse direction the direction bit is toggled. During each positive edge of the clocksignal the register shifts the bit pattern once to the right or left. This shifting action causes thestepper motor to move one step. The darlington sink drivers have an incorporated free wheelingdiode to minimize voltage spikes caused by each coil of the stepper motor. In order to achievehigher torque the shift registers rotate with the bit pattern ‘0011’ instead of ‘0001’. This patternallows two coils to be energized at a time which provides more torque but consumes twice asmuch power.

11

Figure 7. Driving circuit for one stepper motor.

4.3 Interfacing Electronics with Computer Parallel Port

To reduce the number of control lines a 1:4 decoder/demultiplexer is used to share a single clocksignal. However, some motors need to be running simultaneously and have been provided adedicated clock signal. The mode select input lines S0 and S1 are ‘01’ to rotate left and ‘10’ torotate right. To reduce the number of control lines an inverter can be used to invert S0 andsupplied to the S1 input. However, at power up the shift register must be loaded with the defaultbit pattern by asserting both S0 and S1 at the same time. To accommodate this scenario theinverter was replaced with a 2 input NAND gate that is used as a selectable inverter. The secondinput to the NAND gate is low to parallel load the default bit pattern. When the second input ofthe NAND gate is high the output is the opposite of S0. A block box diagram of the inputs to theelectronics can be seen in Figure 8 and a complete schematic is available in the Appendix.

Figure 8. A black box representation of the inputs and outputs of the electronics

12

4.4 Building Circuit

The robotic arm circuitry was built onto a prototype board as shown in Figure 9. The upper leftcorner holds the darlington sink drivers that provide 12V to the stepper motors. The lower railconsists of 6 shift registers using 5V power. The upper right corner is where the 1:4 decoder andNAND gates are located.

Figure 9. Robot arm circuitry built onto a prototype board.

Power was supplied to the circuit by a 12V wall adapter. A LM317 voltage regulator was usedon the circuit to reduce the 12V down to 5V for all the digital logic. A schematic of the LM317configuration required to provide 5V is shown in Figure 10.

13

Figure 10. LM317 voltage regulator providing a regulated 5V to the circuit.

14

5 Software

5.1 Introduction

The software was implemented using Microsoft Visual C++ under Windows 98. A DB-25parallel port was used for communicating with the robot via the electronics. The software had tosupply all operational data for the robotic arm including:

o Resetting the robot arm by sending signal to load shift registers with default valueo Selecting which motor moves and by how much by sending it the appropriate number of

clock pulseso Selecting direction of each stepper motor

Rotating up to three motors of the robotic arm simultaneously to keep the arm in its desiredposition.

5.2 Software Features

The software was written in Visual C++ to create a easy to use graphical user interface. Ascreenshot of the software interface can be seen in Figure 11. A control box for each motor isavailable where the number of steps and the direction can be entered. In addition to providing aninterface for the user, the software can save and retrieve the robotic arms motions. The numberof steps and direction of each motor can be stored in a text file. The data is stored in a commadelimited file format and the robot arm software reads the motor number, direction and numberof steps to execute the saved task.

Figure 11. Screenshot of robotic arm control software written in Visual C++.

15

5.3 Function Descriptions

The robot arm control software is contained in the appendix. A flowchart of the software isshown in Figure 12.

Figure 12: Robotic arm software flowchart.

The MoveMotor function shown below is the function that controls motion of the robot arm.

void MoveMotor(int motor, int dir, int steps){int select_motor = SelectMotor(motor);int set_clock = SetDirection(dir, select_motor, motor);StepMotor(steps, set_clock, motor);

}

MoveMotorThis function is called when the user selects the “Rotate Motor” button as seen in Figure 11. It isalso called when a text file is loaded into the listview box and the user selects “Start.” The mainparameters that the function uses are passed in as integers; they let the program know whichmotor to select, how many steps to apply, and which respective direction to move in.

SelectMotorThis function selects the appropriate data bits (D1,D0) to be written to the parallel port. Thesedata bits, when written to the port, select which motors to operate. Only one parameter, themotor number as integer, is passed into SelectMotor. This function does not set the bits itself,but simply returns a value, which is passed into the SetDirection function.

16

SetDirectionThis function writes the data bits to the parallel port. Not only are the direction bits written (D2-D7), but the bits, which were passed into the function as select_motor are also written to theparallel port. This function also has the important role of setting the appropriate control bits tobe returned. The control bits (C0,C1,C3) correspond to the generic clock, and the two additionaldedicated clocks for motors 1 and 2, respectively.

StepMotorAs the name implies, this functions steps the motors given that all information relating to theelectronics has been set. The function sets up an operating frequency for the stepper motorswhich is processor speed dependant. In order to rotate a motor, a clock pulse must be sent to theappropriate control bit that has been set in the SetDirection function.

17

6 Discussion

The goal of this project was to build a computer controlled robot arm that could move threewooden blocks into a pyramid formation. As shown in Figure 13 the project was a success. Thetoy robot arm from Radio Shack was successfully converted into a computer controlled robotarm. Six stepper motors were put into the base of the robot arm to drive different parts of thearm.

Figure 13. Computer controlled robot arm moving blocks in pyramid formation.

The major hurdle in this project was finding a method of fitting all six stepper motors into thebase of the robot arm. The gears that drive the robot arm could not be changed and it tooknumerous tries and hours of planning to find a position to access the gears and also be out of theway of the other motors. During the automation of the arm it was discovered that during somearm movements other parts of the arm moved as well. To compensate for the movement somemotors were driven simultaneously to readjust for the gearing mechanisms.Other approaches to this computer controlled robot arm included providing control through acomputer’s USB port instead of the parallel port. Since the USB port is a serial device a largeshift register could be used to store all the output bits. Every so often the entire shift register isrefreshed allowing the bits to change and the robot arm to be moved. Other robot arms have themotors mounted onto the arm itself instead of onto the base. That option was not available forthis robot arm. However acquisition of a different type of robot arm may allow for that a create arobotic arm with even more accuracy.

18

7 References

Arm Your Atari - Part 1: http://retrobits.net/armyouratari1.html

Arming your Robot: http://www.lmsm.info/back-issues/0303/arms.html

Features of Industrial Robots: http://www.io.com/~hcexres/tcm1603/acchtml/class_ex.html

Jones on Stepping Motors: http://www.cs.uiowa.edu/~jones/step/

PC Parallel Port - Reading & Writing Data: http://www.doc.ic.ac.uk/~ih/doc/par/doc/data.html

PC's Parallel Port: http://www.lvr.com/parport.htm

Stepper Motor Controller: http://www.aaroncake.net/circuits/stepper.htm

Stepper Motors: http://www.doc.ic.ac.uk/~ih/doc/stepper/

Types of Robots: http://prime.jsc.nasa.gov/ROV/types.html

Using a PC's parallel port for more than printers: http://www.arunet.co.uk/tkboyd/ele1pp.htm

19

Appendix A – Schematic Diagram

20

Appendix B – Source Code

#include "stdafx.h"#include "conio.h"#include "Fstream.h"#include "iostream.h"#include "string.h"

#define DATA 0x378 /* lp1 */#define STATUS 0x379 /* lp1 */#define CONTROL 0x37a /* lp1 */

int SelectMotor(int motor);int SetDirection(int dir, int select_motor, int motor);void ResetRobot();void ResetNot();void StepMotor(int steps, int set_clock, int motor);void MoveMotor(int motor, int dir, int steps);//void WriteFile(int motor, int dir, int steps);void SaveFile(int mMotor, int mDir, int mSteps);

//Motor Description Max Frequency(Hz)//1 Gripper 495//2 Wrist up/down 250//3 Main arm tilt up/down 490//4 Middle arm left/right 490//5 Wrist rotation (360 degrees) 500//6 Main arm rotation (360 degrees) 485

int nD0 = 1;int nD1 = 2;int nD2 = 4;int nD3 = 8;int nD4 = 16;int nD5 = 32;int nD6 = 64;int nD7 = 128;

int nC0 = 1;int nC1 = 2;int nC2 = 4;int nC3 = 8;

int motorSelect;int setPortData;int setClock;int Tmax;

int SelectMotor(int motor) {/*

21

Motor D0 D11 - - CLOCK C12 - - CLOCK C33 0 0 CLOCK C04 0 1 CLOCK C05 1 0 CLOCK C06 1 1 CLOCK C0*/

switch(motor) { case 1:

motorSelect = 0; return motorSelect; case 2: //(0 1) => (D1 D0)=> MOTOR 5 motorSelect = nD0; return motorSelect; case 3: //(0 0) => (D1 D0)=> MOTOR 3 motorSelect = 0;

return motorSelect; case 4: //(1 0) => (D1 D0)=> MOTOR 4 motorSelect = nD1; return motorSelect; case 5: //(0 1) => (D1 D0)=> MOTOR 5 motorSelect = nD0;

return motorSelect; case 6: //(1 1) => (D1 D0)=> MOTOR 6 motorSelect = nD0 + nD1;

return motorSelect;}return 0;

}

int SetDirection(int dir, int select_motor, int motor) {if (dir == 1){

switch(motor) { case 1: setPortData = select_motor +nD2;

_outp(DATA, setPortData);setClock = nC1;return setClock;

case 2:/*Motors 1 & 5 must be compensated in order for Motor 2

to move positions accuratelynD3 => Motor 2 Direction HIGHnD2 => Motor 1 Direction LOWnD6 => Motor 5 Direction LOWnC0 => Motor 5 Clock

22

nC1 => Motor 1 ClocknC3 => Motor 2 Clock

Direction of Motor 1 & 5 is Opposite therefore don'tinclude in setPortData

Include in else statement => setPortData =select_motor + nD2 + nD6;

*/setPortData = select_motor + nD3;_outp(DATA, setPortData);setClock = nC3 + nC0 + nC1;return setClock;

case 3:setPortData = select_motor + nD4;_outp(DATA, setPortData);setClock = nC0;return setClock;


case 5: /*

Motors 1 must be compensated in order for Motor 5 to move positionsaccurately

nD6 => Motor 5 DirectionnC0 => Motor 5 ClocknC1 => Motor 1 Clock*/setPortData = select_motor +nD6;_outp(DATA, setPortData);setClock = nC0 + nC1;return setClock;


}return 0;

}else {

switch(motor) { case 1:

setPortData = select_motor;_outp(DATA, setPortData);setClock = nC1;return setClock;

case 2:/*

23

Motors 1 & 5 must be compensated in order for Motor 2 to movepositions accurately

nD3 => Motor 2 Direction LOWnD2 => Motor 1 Direction HIGHnD6 => Motor 5 Direction HIGHnC0 => Motor 5 ClocknC1 => Motor 1 ClocknC3 => Motor 2 Clock*/setPortData = select_motor + nD2 + nD6;_outp(DATA, setPortData);setClock = nC0 + nC1 + nC3;return setClock;

case 3:setPortData = select_motor;_outp(DATA, setPortData);setClock = nC0;return setClock;


case 5:/* Motors 1 must be compensated in order for Motor 5 to move positionsaccurately

nD2 => Motor 1 DirectionnC0 => Motor 5 ClocknC1 => Motor 1 Clock */setPortData = select_motor + nD2;_outp(DATA, setPortData);setClock = nC0 + nC1;return setClock;


}return 0;

}}void ResetRobot(){

_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7);ResetNot();

}

void ResetNot(){_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7);_outp(CONTROL, 0); //ResetNotSleep(10); //wait

24

_outp(CONTROL, nC1 + +nC2 + nC3); //ResetNotint select_motor3 = SelectMotor(3);_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 + select_motor3);_outp(CONTROL, nC1 + nC3); //ResetNot

Sleep(10); //wait_outp(CONTROL, nC0 + nC1 + nC3); //ResetNotint select_motor4 = SelectMotor(4);_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 + select_motor4);_outp(CONTROL, nC1 + nC3); //ResetNot

Sleep(10); //wait_outp(CONTROL, nC0 + nC1 + nC3); //ResetNotint select_motor5 = SelectMotor(5);_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 + select_motor5);_outp(CONTROL, nC1 + nC3); //ResetNotSleep(10); //wait_outp(CONTROL, nC0 + nC1 + nC3); //ResetNotint select_motor6 = SelectMotor(6);_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 + select_motor6);_outp(CONTROL, nC1 + nC3); //ResetNotSleep(10); //wait_outp(CONTROL, nC0 + nC1 + nC3); //ResetNot

_outp(CONTROL, nC2 + nC0 + nC1 + nC3 ); //ResetNot}void StepMotor(int steps, int set_clock, int motor){

switch(motor) { case 1: Tmax = 2; // Max frequency = 495Hz break; case 2: Tmax = 3; // Max frequency = 250Hz break; case 3: Tmax = 2; // Max frequency = 250Hz break; case 6: Tmax = 3; // Max frequency = 250Hz break; default: Tmax = 2; // Max frequency = 485Hz break; }

int counter; for(counter = 0; counter < steps; ++counter) {

switch(motor) {

case 2: if (counter >= steps*.87) {

25

outp(CONTROL, set_clock + nC2); //Apply strobe withresetNot = normal operation (nC2) Sleep(Tmax);

_outp(CONTROL, nC2);

}else {outp(CONTROL, set_clock + nC2 -nC1 -nC0); //Apply strobe

with resetNot = normal operation (nC2) Sleep(Tmax);

_outp(CONTROL, nC2);

} break;

default:_outp(CONTROL, set_clock + nC2); //Apply strobe with

resetNot = normal operation (nC2) Sleep(Tmax);

_outp(CONTROL, nC2); break;

}}}

void MoveMotor(int motor, int dir, int steps){// ResetRobot();

int select_motor = SelectMotor(motor);int set_clock = SetDirection(dir, select_motor, motor);StepMotor(steps, set_clock, motor);

}void SaveFile(int mMotor, int mDir, int mSteps){char strComma[] = ",";char strSemi[] = ",";

ofstream SaveFile("robot.txt", ios::ate); //ios::ate => end of fileSaveFile << mMotor << strComma << mDir << strComma << mSteps <<strSemi << endl;SaveFile.close();}

computer controlled robot arm - uvic department of electrical and

Documents