Computer Controlled Robot Arm

Transcription

University of Victoria
Faculty of Engineering
Summer 2004 ELEC499A
Computer Controlled
Robot Arm
http://web.uvic.ca/~vickayak/web/
Supervisor:
Dr. Subhasis Nandi
Group Members:
Mehran Haji Rasouliha
Dylan Sproule
Jason Wong
Table of Contents
LIST OF TABLES ..................................................................................................................................................... iii
LIST OF FIGURES................................................................................................................................................... iii
1
PROJECT OVERVIEW................................................................................................................................... 1
1.1
1.2
1.3
1.4
1.5
1.6
2
ROBOTIC ARM ............................................................................................................................................... 4
2.1
2.2
2.3
3
INTRODUCTION................................................................................................................................................ 6
TYPES OF ELECTRIC MOTORS.......................................................................................................................... 6
MOTORS USED IN PROJECT ............................................................................................................................. 8
ELECTRONICS.............................................................................................................................................. 10
4.1
4.2
4.3
4.4
5
INTRODUCTION................................................................................................................................................ 4
TYPES OF ROBOTIC ARMS ............................................................................................................................... 4
PROJECT ROBOT ARM ..................................................................................................................................... 5
MECHANICS.................................................................................................................................................... 6
3.1
3.2
3.3
4
INTRODUCTION................................................................................................................................................ 1
CRS-PLUS ROBOTIC SYSTEM .......................................................................................................................... 1
PROJECT GOAL ................................................................................................................................................ 1
DIVISION OF TASKS ......................................................................................................................................... 2
TASK COMPLETION TIMETABLE ...................................................................................................................... 2
PROJECT PART SOURCES ................................................................................................................................. 3
INTRODUCTION.............................................................................................................................................. 10
STEPPER MOTOR DRIVING CIRCUIT .............................................................................................................. 10
INTERFACING ELECTRONICS WITH COMPUTER PARALLEL PORT ................................................................... 11
BUILDING CIRCUIT ........................................................................................................................................ 12
SOFTWARE.................................................................................................................................................... 14
5.1
5.2
5.3
INTRODUCTION.............................................................................................................................................. 14
SOFTWARE FEATURES ................................................................................................................................... 14
FUNCTION DESCRIPTIONS.............................................................................................................................. 15
6
DISCUSSION .................................................................................................................................................. 17
7
REFERENCES ................................................................................................................................................ 18
Appendix A – Schematic Diagram
Appendix B – Source Code
ii
List of Figures
Figure 1. Three square wooden blocks moved from stacked formation into pyramid formation. . 1
Figure 2. The Armatron toy robot arm made by Radio Shack in the 1980’s................................. 5
Figure 3. The original DC motor used to drive all gearing mechanisms........................................ 6
Figure 4. 5 wire unipolar stepper motor and movement of shaft.................................................... 7
Figure 5. Fittings made for each stepper motor out of PVC rod with gear attached to end. .......... 8
Figure 6. Stepper motors with gear fittings placed onto chassis of robot arm................................ 9
Figure 7. Driving circuit for one stepper motor............................................................................ 11
Figure 8. A black box representation of the inputs and outputs of the electronics....................... 11
Figure 9. Robot arm circuitry built onto a prototype board.......................................................... 12
Figure 10. LM317 voltage regulator providing a regulated 5V to the circuit............................... 13
Figure 11. Screenshot of robotic arm control software written in Visual C++............................ 14
Figure 12: Robotic arm software flowchart. ................................................................................. 15
Figure 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. ...................................................... 2
Table 2. Completion date of project tasks. ..................................................................................... 2
Table 3. Stepper motor number and part of robot arm that it controls. .......................................... 8
Table 4. Description of parallel port connections and purpose. ................................................... 10
iii
1
1.1
Project Overview
Introduction
The objective of our ELEC 499 project is to design and build a computer controlled robot arm
with similar functionality to the commercially available CRS-Plus robot arm. The team
members 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 standard
hand, a system controller, and a teach pendant. The mechanical manipulator uses servomotors to
move in five degrees of freedom. The teach pendant is used to move the robot arm manually
joint by joint. The CRS-Plus is initially taught manually with the teach pendant. Once all the
points are saved the system controller can move the arm and repeat the motions with a high
degree of accuracy and precision.
1.3
Project Goal
Unfortunately, the CRS-Plus robotic system costs thousands of dollars to purchase. The goal of
this project is to create a computer controlled robot arm with similar functionality which costs
much less. The scope of the project includes the designing and building of the hardware and
software for a comparable robot arm. The completed project will consist of a robot arm with five
degrees 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 perform
the same task as required of the CRS-Plus robot arm in ELEC 360’s Laboratory #4. The task is
to move three square wooden blocks stacked on top of each other into a pyramid formation as
shown in Figure 1.
Figure 1. Three square wooden blocks moved from stacked formation into pyramid formation.
1
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
Responsibility
Mechanics
Mehran, Jason
? Removing of levers and gears
? Creating fittings for stepper motors
? Implementing motors into arm
chassis
Jason, Dylan
Electronics
? Designing control electronics for
stepper motors
? Prototyping electronics and building
circuit board
Software
Dylan, Jason
? Accessing parallel port
? Creating control signals to drive
stepper motors
Jason, Dylan, Mehran
Report
? Documentation of project
? Schematics and flowcharts
Webpage
Dylan, Jason
? Design and creation of report
contents into a web site
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
Project definition
Hardware design
Hardware creation
Electronics design
Electronics creation
Software design
Software creation
Connecting hardware and software components
Testing and calibration
Project completion and poster presentation
Final report and webpage submitted
Completion Date
May 21, 2004
May 21, 2004
June 4, 2004
June 11, 2004
June 18, 2004
June 25, 2004
July 9, 2004
July 16, 2004
July 20, 2004
July 23, 2004
July 30, 2004
2
1.6
Project Part Sources
The project parts are listed in Table 3.
Table 3. Parts acquired for completion of project.
Qty
Description
6 IC 4bit bi-direction shift register 16-DIP
4 IC driver darlington 7ch 50V .5A 16DIP
1 12V zener diode 1W 5% DO-41
2 Quad 2-input NOR gate 14-DIP
1 Dual 1:4 decoder/demultiplexer
1 Radio Shack Armatron Robotic Arm
6 12V Stepper motor, 1.8 deg/step
2 feet 1” diameter solid PVC rod
Part#
CD40194BE
ULN2003AP
1N4742A-T
CD4001UBE
74F139PC
-------
Price
Source
1.29 Digi-Key
0.63 Digi-Key
0.5 Digi-Key
0.7 Digi-Key
0.35 Digi-Key
6.99 Value Village
0.00 5 ¼” floppy drives
20.00 Industrial Plastic
and Paints
3
2
2.1
Robotic Arm
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 the
type 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 a
different work envelope. The number of axis is directly related to the maneuverability of a
robotic arm.
Revolute Coordinate Robot Arm
The revolute coordinate robot arm is very similar to the human arm and it is capable of many of
the same motions as a human arm. But the design of the this kind of robot is a little bit different
from human arm due to the complexity of the human shoulder joint. The shoulder of the
Revolute robot rotates by spinning the arm at its base. The movement of the shoulder is done by
flexing the upper arm member back and forth while the elbow joint moves the forearm up and
down. This kind of robot is very flexible and looks somewhat like a human arm.
Polar Coordinate Robot Arm
The polar coordinate robot arm is very flexible and can grasp different kinds of objects around
the robot. The robot rotates by a turntable base and the elbow joint is the second degree of
freedom and moves the forearm up and down. This robot achieves the third degree of freedom by
changing the reach of the forearm. The inner forearm has the job of bringing the gripper close or
away from the robot.
Cylindrical Coordinate Robot Arm
The cylindrical coordinate robot arm has the shape of a robotic forklift. The area which this arm
works in is the shape of a thick cylinder. The rotation of the shoulder is done by revolving the
base like the polar coordinate system. The forearm of this robot can grasp objects of different
heights by moving the forearm up and down the column. The forearm also has a three
dimensional work envelope.
Cartesian Coordinate Robot Arm
The Cartesian coordinate robot arm consists of a carrier belt like track that makes the arm go
back and forth. The work envelope of this robot arm is shaped like a box. The forearm of the
robot moves up and down along the column and contains an inner arm that can reach both close
and far.
4
2.3
Project Robot Arm
The project’s robot arm will originate from an Armatron toy robot arm shown in Figure 2. It was
made by Radio Shack and Tandy in the 1980’s. The robot arm was acquired from the second
hand 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 is
originally controlled by moving the two joysticks which in turn engage a DC motor to different
gearing mechanisms. It was decided to use an already fabricated arm because it made the project
less time consuming and cheaper. The ultimate goal of the project is more control oriented than
manufacturing oriented.
Figure 2. The Armatron toy robot arm made by Radio Shack in the 1980’s
5
3
3.1
Mechanics
Introduction
The toy robot arm was originally controlled by moving the two joysticks. The two joysticks
would 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 motors
available 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 motors
AC motors are commonly found in industrial factories driving conveyor belts or other heavy
machinery. There are very few robot arms that use AC motors. Some of the reasons are due to
poor accuracy, low starting torque and poor dynamic response.
6
Continuous DC motors
DC motors are more commonly found in robot arms. Continuous motors move as long as power
is applied and stops as soon as the power is removed. For continuous rotation the power has to
be pulsed to the motor and this is done by a computer or indexer. Different types of continuous
motors include brushless, permanent magnet and variable reluctance. However, these motors do
not provide position control and are not very useful for precise robotic arm movement.
Servo motors
A servo motor is a DC motor combined with some position sensing parts. Typically servo
motors have 3 wires coming out from the motor. Two lines are for power and the third line is a
control input. A pulse width signal applied to this input tells the motor to what position it should
be moved to. The inside of a servo motor consists of a DC motor, a geartrain, limits stops and a
potentiometer for position feedback. Robot arms using servos monitor the robot’s axes and its
components for position and velocity.
Stepper motors
A stepper motor is very simple DC motor because it has no brushes or contacts. It operates by
having its magnetic field electronically switched to rotate the armature magnet around. This
setup allows the motor to rotate a specific angle and stop. There are two types of stepper motors
which are bipolar and unipolar types. The bipolar stepper motor consists of two coils and the
current direction is reversed in each coil to achieve four separate positions. The other type of
stepper motor is a unipolar type and is shown in Figure 4. The unipolar stepper motor consists of
four coils. When each coil is energized individually and in proper sequence the motor shaft turns
the specified number of degrees per step.
Figure 4. 5 wire unipolar stepper motor and movement of shaft.
7
3.3
Motors Used in Project
Stepper motors were chosen for this project because of their easy circuit implementation and
availability. The stepper motors for this project were taken from old 5 ¼” computer floppy
drives. The read/write heads used stepper motors to move back and forth along the magnetic
disk. The stepper motors did not cost any money as they were salvaged from old computers and
junk piles.
To allow the robot arm to be computer controlled the DC motor was removed and replaced with
six stepper motors. The system controller software being run on a computer will control and
regulate the number of steps of each motor. The motors were all placed into the base of the robot
arm because that is where all the mechanisms and driving gears were. The steppers motors were
put directly in line with the gears in the base with the aid of some fittings. The fittings were
made for each stepper motor using a 1” solid PVC rod as shown in Figure 5. At the end of each
rod a ¾” hole was drilled to hold the shaft of the motor. A pilot hole was made at the other end
of 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 Figure
6. 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
Gripper
2
Wrist up/down
3
Main arm tilt up/down
4
Middle arm left/right
5
Wrist rotation (360 degrees)
6
Main arm rotation (360 degrees)
8
Figure 6. Stepper motors with gear fittings placed onto chassis of robot arm.
9
4
4.1
Electronics
Introduction
The stepper motors are controlled through the computer’s parallel port. A separate control
circuit was created to minimize the number of control lines required of the computer and also to
provide 12V power to each separate coil of the motor. A description of each input to the
electronics circuit and connections to the parallel are summarized in Table 4.
Table 4. Description of parallel port connections and purpose.
Name
Description
Generic Clock
Squarewave generated by computer,
frequency controls speed of motor
Motor 1 Clock
Drives Motor 1
Motor 2 Clock
Drives Motor 2
ResetNot
Initialize shift registers with default value
Motor Select
Selects which motor receives clock input
Motor X Direction
Controls direction of motor
4.2
Parallel Port Pin
1 (C0 - Inverted)
14 (C1 - Inverted)
17 (C3 - Inverted)
16 (C2)
3,2 (D1,D0)
9,8,7,6,5,4 (D7-D2)
Motor 6 = D7, …,
Motor 1 = D2
Stepper Motor Driving Circuit
The stepper motors used in this project are 5 wire unipolar motors. Each stepper motor has its
own driving circuit consisting of a 4 bit shift register and four darlington sink drivers as shown in
Figure 7. A total of 6 circuits as shown in Figure 7 were produced. To drive each stepper motor
the shift register must first be loaded with an initial bit pattern by setting direction high and
ResetNot low. After initialization a clock signal is fed into the shift register and a direction is
chosen. To reverse direction the direction bit is toggled. During each positive edge of the clock
signal the register shifts the bit pattern once to the right or left. This shifting action causes the
stepper motor to move one step. The darlington sink drivers have an incorporated free wheeling
diode to minimize voltage spikes caused by each coil of the stepper motor. In order to achieve
higher torque the shift registers rotate with the bit pattern ‘0011’ instead of ‘0001’. This pattern
allows two coils to be energized at a time which provides more torque but consumes twice as
much power.
10
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 clock
signal. However, some motors need to be running simultaneously and have been provided a
dedicated clock signal. The mode select input lines S0 and S1 are ‘01’ to rotate left and ‘10’ to
rotate right. To reduce the number of control lines an inverter can be used to invert S0 and
supplied to the S1 input. However, at power up the shift register must be loaded with the default
bit pattern by asserting both S0 and S1 at the same time. To accommodate this scenario the
inverter was replaced with a 2 input NAND gate that is used as a selectable inverter. The second
input to the NAND gate is low to parallel load the default bit pattern. When the second input of
the NAND gate is high the output is the opposite of S0. A block box diagram of the inputs to the
electronics 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
11
4.4
Building Circuit
The robotic arm circuitry was built onto a prototype board as shown in Figure 9. The upper left
corner holds the darlington sink drivers that provide 12V to the stepper motors. The lower rail
consists of 6 shift registers using 5V power. The upper right corner is where the 1:4 decoder and
NAND 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 used
on the circuit to reduce the 12V down to 5V for all the digital logic. A schematic of the LM317
configuration required to provide 5V is shown in Figure 10.
12
Figure 10. LM317 voltage regulator providing a regulated 5V to the circuit.
13
5
5.1
Software
Introduction
The software was implemented using Microsoft Visual C++ under Windows 98. A DB-25
parallel port was used for communicating with the robot via the electronics. The software had to
supply all operational data for the robotic arm including:
o Resetting the robot arm by sending signal to load shift registers with default value
o Selecting which motor moves and by how much by sending it the appropriate number of
clock pulses
o Selecting direction of each stepper motor
Rotating up to three motors of the robotic arm simultaneously to keep the arm in its desired
position.
5.2
Software Features
The software was written in Visual C++ to create a easy to use graphical user interface. A
screenshot of the software interface can be seen in Figure 11. A control box for each motor is
available where the number of steps and the direction can be entered. In addition to providing an
interface for the user, the software can save and retrieve the robotic arms motions. The number
of steps and direction of each motor can be stored in a text file. The data is stored in a comma
delimited file format and the robot arm software reads the motor number, direction and number
of steps to execute the saved task.
Figure 11. Screenshot of robotic arm control software written in Visual C++.
14
5.3
Function Descriptions
The robot arm control software is contained in the appendix. A flowchart of the software is
shown 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);
}
MoveMotor
This function is called when the user selects the “Rotate Motor” button as seen in Figure 11. It is
also called when a text file is loaded into the listview box and the user selects “Start.” The main
parameters that the function uses are passed in as integers; they let the program know which
motor to select, how many steps to apply, and which respective direction to move in.
SelectMotor
This function selects the appropriate data bits (D1,D0) to be written to the parallel port. These
data bits, when written to the port, select which motors to operate. Only one parameter, the
motor 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.
15
SetDirection
This function writes the data bits to the parallel port. Not only are the direction bits written (D2D7), but the bits, which were passed into the function as select_motor are also written to the
parallel port. This function also has the important role of setting the appropriate control bits to
be returned. The control bits (C0,C1,C3) correspond to the generic clock, and the two additional
dedicated clocks for motors 1 and 2, respectively.
StepMotor
As the name implies, this functions steps the motors given that all information relating to the
electronics has been set. The function sets up an operating frequency for the stepper motors
which is processor speed dependant. In order to rotate a motor, a clock pulse must be sent to the
appropriate control bit that has been set in the SetDirection function.
16
6
Discussion
The goal of this project was to build a computer controlled robot arm that could move three
wooden blocks into a pyramid formation. As shown in Figure 13 the project was a success. The
toy robot arm from Radio Shack was successfully converted into a computer controlled robot
arm. Six stepper motors were put into the base of the robot arm to drive different parts of the
arm.
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 the
base of the robot arm. The gears that drive the robot arm could not be changed and it took
numerous tries and hours of planning to find a position to access the gears and also be out of the
way of the other motors. During the automation of the arm it was discovered that during some
arm movements other parts of the arm moved as well. To compensate for the movement some
motors were driven simultaneously to readjust for the gearing mechanisms.
Other approaches to this computer controlled robot arm included providing control through a
computer’s USB port instead of the parallel port. Since the USB port is a serial device a large
shift register could be used to store all the output bits. Every so often the entire shift register is
refreshed allowing the bits to change and the robot arm to be moved. Other robot arms have the
motors mounted onto the arm itself instead of onto the base. That option was not available for
this robot arm. However acquisition of a different type of robot arm may allow for that a create a
robotic arm with even more accuracy.
17
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
18
Appendix A – Schematic Diagram
19
Appendix B – Source Code
#include
#include
#include
#include
#include
"stdafx.h"
"conio.h"
"Fstream.h"
"iostream.h"
"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
(Hz)
//1
//2
//3
//4
//5
//6
Description
Gripper
Wrist up/down
Main arm tilt up/down
Middle arm left/right
Wrist rotation (360 degrees)
Main arm rotation (360 degrees)
int
int
int
int
int
int
int
int
nD0
nD1
nD2
nD3
nD4
nD5
nD6
nD7
=
=
=
=
=
=
=
=
1;
2;
4;
8;
16;
32;
64;
128;
int
int
int
int
nC0
nC1
nC2
nC3
=
=
=
=
1;
2;
4;
8;
int
int
int
int
motorSelect;
setPortData;
setClock;
Tmax;
Max Frequency
495
250
490
490
500
485
int SelectMotor(int motor) {
/*
20
Motor
1
2
3
4
5
6
*/
D0
0
0
1
1
D1
0
1
0
1
CLOCK
CLOCK
CLOCK
CLOCK
CLOCK
CLOCK
C1
C3
C0
C0
C0
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 accurately
nD3 => Motor 2 Direction HIGH
nD2 => Motor 1 Direction LOW
nC0 => Motor 5 Clock
21
Direction of Motor 1 & 5 is Opposite therefore don't
include in setPortData
Include in else statement => setPortData =
select_motor + nD2 + nD6;
*/
setPortData = select_motor + nD3;
setClock = nC3 + nC0 + nC1;
return setClock;
case 3:
setClock = nC0;
return setClock;
case 4:
setClock = nC0;
return setClock;
case 5:
/*
Motors 1 must be compensated in order for Motor 5 to move positions
accurately
nD6 => Motor 5 Direction
*/
setPortData = select_motor +nD6;
setClock = nC0 + nC1;
return setClock;
case 6:
setClock = nC0;
return setClock;
}
return 0;
}
else {
switch(motor) {
case 1:
setPortData = select_motor;
setClock = nC1;
return setClock;
case 2:
/*
22
Motors 1 & 5 must be compensated in order for Motor 2 to move
positions accurately
*/
setPortData = select_motor + nD2 + nD6;
setClock = nC0 + nC1 + nC3;
return setClock;
case 3:
setClock = nC0;
return setClock;
case 4:
setClock = nC0;
return setClock;
case 5:
/* Motors 1 must be compensated in order for Motor 5 to move positions
accurately
nD2 => Motor 1 Direction
*/
setClock = nC0 + nC1;
return setClock;
case 6:
setClock = nC0;
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); //ResetNot
Sleep(10);
//wait
23
_outp(CONTROL, nC1 + +nC2 + nC3); //ResetNot
int select_motor3 = SelectMotor(3);
_outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7
_outp(CONTROL, nC1 + nC3); //ResetNot
Sleep(10);
//wait
_outp(CONTROL, nC0 + nC1 + nC3); //ResetNot
Sleep(10);
//wait
Sleep(10);
//wait
Sleep(10);
//wait
+ select_motor3);
+ select_motor4);
+ select_motor5);
+ select_motor6);
_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;
break;
case 3:
Tmax = 2;
break;
case 6:
Tmax = 3;
break;
default:
Tmax = 2;
break;
}
int counter;
for(counter = 0; counter < steps; ++counter)
{
switch(motor) {
case 2:
if (counter >= steps*.87) {
24
outp(CONTROL, set_clock + nC2); //Apply strobe with
resetNot = 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);
}
break;
default:
_outp(CONTROL, set_clock + nC2); //Apply strobe with
resetNot = normal operation (nC2)
Sleep(Tmax);
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 file
SaveFile << mMotor << strComma << mDir << strComma << mSteps <<
strSemi << endl;
SaveFile.close();
}
25

Computer Controlled Robot Arm

Transcription

Similar documents

COMMERCIAL BOOTHS

SaeboFlex / SaeboReach Brochure

LR Mate 200iD pdf

Installation of Robo-Focus Motor

Make yourself comfortable

New York Arm Wrestling® and Mulcahy`s Pub Presents the

Honda 300ex National Race Machine

WhisperShade® IQ® System

CNC Router Project - Home Metal Shop Club

Engine Downsizing Potential of Electric Turbochargers