Computer Controlled Robot Arm
Transcription
Computer Controlled Robot Arm
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 nD6 => Motor 5 Direction LOW nC0 => Motor 5 Clock 21 nC1 => Motor 1 Clock nC3 => Motor 2 Clock 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; _outp(DATA, setPortData); setClock = nC3 + nC0 + nC1; return setClock; case 3: setPortData = select_motor + nD4; _outp(DATA, setPortData); setClock = nC0; return setClock; case 4: setPortData = select_motor + nD5; _outp(DATA, setPortData); setClock = nC0; return setClock; case 5: /* Motors 1 must be compensated in order for Motor 5 to move positions accurately nD6 => Motor 5 Direction nC0 => Motor 5 Clock nC1 => Motor 1 Clock */ setPortData = select_motor +nD6; _outp(DATA, setPortData); setClock = nC0 + nC1; return setClock; case 6: setPortData = select_motor + nD7; _outp(DATA, setPortData); setClock = nC0; return setClock; } return 0; } else { switch(motor) { case 1: setPortData = select_motor; _outp(DATA, setPortData); setClock = nC1; return setClock; case 2: /* 22 Motors 1 & 5 must be compensated in order for Motor 2 to move positions accurately nD3 => Motor 2 Direction LOW nD2 => Motor 1 Direction HIGH nD6 => Motor 5 Direction HIGH nC0 => Motor 5 Clock nC1 => Motor 1 Clock nC3 => 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 4: setPortData = select_motor; _outp(DATA, setPortData); setClock = nC0; return setClock; case 5: /* Motors 1 must be compensated in order for Motor 5 to move positions accurately nD2 => Motor 1 Direction nC0 => Motor 5 Clock nC1 => Motor 1 Clock */ setPortData = select_motor + nD2; _outp(DATA, setPortData); setClock = nC0 + nC1; return setClock; case 6: setPortData = select_motor; _outp(DATA, setPortData); 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 int select_motor4 = SelectMotor(4); _outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 _outp(CONTROL, nC1 + nC3); //ResetNot Sleep(10); //wait _outp(CONTROL, nC0 + nC1 + nC3); //ResetNot int select_motor5 = SelectMotor(5); _outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 _outp(CONTROL, nC1 + nC3); //ResetNot Sleep(10); //wait _outp(CONTROL, nC0 + nC1 + nC3); //ResetNot int select_motor6 = SelectMotor(6); _outp(DATA, nD2 + nD3 + nD4 + nD5 + nD6 + nD7 _outp(CONTROL, nC1 + nC3); //ResetNot Sleep(10); //wait _outp(CONTROL, nC0 + nC1 + nC3); //ResetNot + 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; // 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) { 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); _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 file SaveFile << mMotor << strComma << mDir << strComma << mSteps << strSemi << endl; SaveFile.close(); } 25