FIVE JOINT ROBOT ARM By Mustafa Serkan Bozkurt Mehmet Fidan. A Graduation Project Report Electrical-Electronics Engineering Department

Transkript

1 FIVE JOINT ROBOT ARM By Mustafa Serkan Bozkurt Mehmet Fidan A Graduation Project Report Electrical-Electronics Engineering Department Haziran 2004

2 2 FIVE JOINT ROBOT ARM by Mustafa Serkan Bozkurt Mehmet Fidan A Report Presented in Partial Fulfillment of the Requirements for the Degree Bachelor of Science in Electrical-Electronics Engineering OSMANGAZI UNIVERSITY Haziran 2004

3 3 FIVE JOINT ROBOT ARM by Mustafa Serkan Bozkurt Mehmet Fidan has been approved Haziran 2004 APPROVED: (Full Name of the Chair of the Committee), Chairperson (Full Name of the other member) Supervisory Committee

4 4 ABSTRACT In this project, a control program is developed for a robot arm that was designed and built by Buket Çelikel Topel. The program is written in C and it is a modified verision of a BASIC program written by Hakan Korul. Since the analytical solution of the inverse kinematics of the robot is not available, inverse kinematic of the robot arm is solved numerically in the program. Required joint angles are determined to reach the entered position in the program. The necessary pulses to rotate the motors are calculated. Compensation pulses which are needed to remove the mechanical constraint problem of the joints are calculated. Finally, the original pulses and the compensation pulses are added and sent to the joints by a PCL 724 interface card. ÖZET Bu projede Buket Çelikel Topel tarafından tasarlanıp gerçekleştirilen robot kolu kullanılmıştır. Bu robot kolu Hakan Korul un yazdığı Basic programı temel alınarak oluşturulan c programıyla kontrol edilmiştir. Programda girilen pozisyona ulaşabilmek için gerekli olan eklem açıları ters kinematik denklemleriyle belirlenmiştir. Motorları döndürmek için gerekli olan sinyal sayıları hesaplandıktan sonra kayışlardan oluşan mekanik yapı problemlerini gidermek için gereken düzeltme sinyalleri hesaplanmıştır. Son olarak ilk hesaplanan sinyal sayılarıyla düzeltme sinyal sayıları eklenip veya çıkarılıp PCL 724 arayüz kartının portlarına gönderilmiştir.

5 5 ACKNOWLEDGEMENT We would like to thank our advisor Dr. Osman PARLAKTUNA for his guidance, support and patience. Also we wish to thank our family for their patience and support during university years.

6 6 TABLE OF CONTENTS page 1. INTRODUCTION 7 2. ROBOT ARM KINEMATICS ROBOT ARM INVERSE KINEMATICS FINDING THE TARGET POINT WITH THE GIVEN COORDINATES PCL-724 INTERFACE CARD CONTROL OF STEP MOTORS EXPLANATION OF SOFTWARE FLOW CHART OF THE PROGRAM EXPLANATION OF SOFTWARE STEP BY STEP CONCLUSION AND SUGGESTIONS REFERENCES APPENDICES THE SOFTWARE FOR CONTROLLING WITH COORDINATES THE SOFTWARE FOR CONTROLLING WITH ANGLES 58

7 7 INTRODUCTION In this project, a control program for a five joint robot arm (Topel) is developed. Topel robot arm consists of 5 step motors and a DC motor. The properties of motors are shown below in figure 1. These motors are driven by 2 driver boards. One of these driver boards is used to drive four of the step motors and the other is used to drive one step motor and the DC motor. The circuit diagrams of these cards are shown in figures 2 and 3. The DC motor is used to open and close the manipulator gripper. This gripper has one inner and one outer switches. Inner switch stops the DC motor when it is completely closed or when it senses an object. The outer switch stops the DC motor when it is completely opened. With the help of these switches, it is not required to send some extra pulses from computer to DC motor. The connection of these switches are shown in figure 4. The 4 step motors are at the bottom of the robot arm and the other step motor and DC motor are on the robot arm. The 4 step motor which are at the bottom move the joints with the help of the belts. The step motor and the DC motor which are at the top moves the fifth joint and the gripper with the help of the gears. These gears and the gears which move the belts provides the joints not to move when the electricity is off. Topel robot arm is shown in picture 1.

8 MOTOR PROPERTIES MOTOR INPUT CONNECTED CABLE OUTPUT 8 Stepper 5 - white blue yellow red Step Syn Stepping Motor Blue Type I 03G I red IBM PN 94X5937 red_white Stepper 4 INSUL. CLASS B bluewhite DC 4.1V 1.1A black 1.8 DEG / STEP white LOT NO - SANYO DENKI Stepper 3 Stepper 2 Stepper 1 CO..LTD. SANYO Step Syn Stepping Motor Type I I DC 4.5V 1.4A DEG / STEP IBM P/N LOT NO - SANYO CO..LTD. SANYO Step Syn Stepping Motor Type I I IBM P/N LOT NO - DC 4.5V 1.4A 2 DEG / STEP DENKI SANYO CO..LTD SANYO Step Syn Stepping Motor Type I I IBM P/N LOT NO - DC 4.5V 1.4A 2 DEG / STEP SANYO CO..LTD DENKI DENKI blue red redwhite bluewhite black white blue red redwhite bluewhite black white blue red redwhite bluewhite black white brown Blue yellow green blue orange brown green silver silver blue orange brown green silver silver blue orange brown green silver silver blue orange Brown yellow silver silver Figure 1(a). The properties and connections of step motors. 1STEP+DCdriver brown pin.7 blue pin.2 yellow pin.6 green pin.1 4XSTEP driver white pin.13 orange pin.15 green pin.16 blue pin XSTEP driver black pin.12 green pin.10 orange pin.9 white pin XSTEP driver signed white pin.5 white pin.7 signed grey pin.8 grey pin XSTEP red pin.4 blue pin.2 black pin.1 green pin.3 - -

9 9 MOTOR End-effector* ( DC motor ) ÖZELLİKLER MOTOR GİRİŞİ - black white BAĞLANAN KABLO Outer switch (II) pink KART ÇIKIŞI 1STEP+DC SÜRÜCÜ - yellow pin.3 Figure 1(b). The properties and connections of DC motor Figure 2. Driver board of 4 step motors Figure 3. Driver board of one step and the DC motor

10 10 Picture 1. Topel Robot Arm Figure 4. schematic of inner and outer switches ROBOT ARM KINEMATICS Robot arm kinematics deals with the analytical study of the geometry of motion of a robot arm with respect to a fixed reference coordinate system without regard to the forces/moments that cause the motion. Thus, kinematics deals with the analytical

11 11 description of the spatial displacement of the robot, in orientation of the end-effector of a robot arm. There are two fundamental problems in robot arm kinematics. The first problem is usually referred to us the direct (or forward) kinematics problem, while the second problem is the inverse kinematics (or arm solution) problem. Since, the independent variables in a robot arm, the joint variables and a task is usually stated in terms of the reference coordinate frame, the inverse kinematics problem used more frequently. ROBOT ARM INVERSE KINEMATICS By using robot arm kinematics the position is found with the given joint angles. But by using inverse kinematics to reach the desired position the joint angles must be determined. Due to having 2 equations but having 3 joints which are dependent each other, there are more than one solution. Therefore, to reach a position each joint angle can not be determined individually. The solution can only be determined by the combination of joint angles. the important thing is that the robot arm must know which solution it must choose. The logical solution is to choose the nearest solution if there is not an obstacle in workspace. The number of solution is dependent on not only the number of joints but also the connection parameters. But in the software, by using the numerical solution of the inverse kinematics the joint angles are determined within an accepted error. FINDING THE TARGET POINT WITH THE GIVEN COORDINATES Cartesian coordinates are entered to the software to make the position control of the robot arm. Schematic of the Topel robot is given in Figure 5.

12 12 Figure 5. Topel Robot arm schematic The X and Z coordinate equations of the robot are given below. X=3+28cos(90-θ 2 )+28.6sin(180-θ 2 -θ 3 )+16sin(180-θ 2 -θ 3 -θ 4 ) Z= sin(90-θ 2 )+28.6sin(180-θ 2 -θ 3 )+16sin(180-θ 2 -θ 3 -θ 4 ) Where θ i represent the ith joint angle of the arm. X θ 1 θ1 = tan 1 Y X Y Figure 6 Determination of the first joint angle The software computes the equations for X and Z and compares them with the entered values. The angle values within the accepted error range are taken as the angles of destination point.

13 13 THE PCL 724 INTERFACE CARD Figure 7 Pin definitions of the PCL 724 card Figure 7 shows the ports of PCL 724 interface card. This interface card transmits the 8- bit data which are sent from computer to A port for controlling the 1st and 2nd step motors, to B port for controlling 3rd and 4th step motors and to C port for controlling 5th step motor and DC motor.to measure the joint angles, mechanism must contain some sensors on each joint. To remove this disadvantage, step motors may be used. Therefore, the joints of the robot arm are controlled using motors. CONTROL OF STEP MOTORS Figure 8 Basic elements of step motor control Control system consists of a PC control program, I/O interface board, driver board, power supply and motors. Interface board receives the signals from control program and sends these low voltage signals to power driver board. Driver board converts these signals to high voltage signals with the power from power supply. EXPLANATION OF THE SOFTWARE The home position of the joints is initialized as q1=0o, q2=0o, q3=90o, q4=0o, q5=0o. First of all, program asks to enter X, Y, Z destination coordinates. Secondly, the destination coordinates are checked whether they are in work space or not. If the coordinates are in work

14 14 space the next step will be executed. Otherwise, the new coordinates are asked. Joint angles are determined with the inverse kinematic equations. All possibilities are tried and the most suitable angles are chosen. After angle calculation, pulses are calculated to reach determined joint angles. The 2 nd, 3 rd and 4 th joints of the robot arm arm coupled through belts and as of theses joints moves, other joints also move. Due to this mechanical constraint of the robot arm, some compensation pulses must be determined. After this processing, these compensation pulses are added to or subtracted from the previously calculated pulses. Added or subtracted pulses are sent to the ports of the interface card. From this interface card, pulses are sent to step motors and the motors begin to run. When the motors reach the desired angles, signals are sent to DC motor to close the gripper. During closing, if the inner switch senses an object it stops the DC motor. If it does not sense an object, DC motor closes the gripper completely. After that program asks whether the user want to return to home position or enter other new coordinates. When the robot reaches to home position or the new coordinates, an opening signal is sent to DC motor to open the gripper.

15 15 Figure 9 Flow Chart of the Program

16 16 EXPLANATION OF THE SOFTWARE STEP BY STEP Başla1: int basla1() {basla(); basla2(); return 0; Basla1 subfunction provides entering new position with calling Başla subfunction, then moves the robot arm to enterred position with calling Başla2 subfunction. Başla: int basla() {if (count>1) devamet(); else referans(); return(0); Basla subfuncion calls Referans subfunction while count variable is smaller than 1. This means the Cartesian coordinates of the first destination is going to be enterred. Otherwise Basla subfunction calls Devamet subfunction. Referans: void referans() {Q11=0; Q22=0; Q32=90.0; Q41=0; Q51=0; R1=Q11; R2=Q22; R3=Q32; R4=Q41; R5=Q51; koordinat(); Referans subfunction initializes the home position and calls Koordinat subfunction. In this subfunction 'Q11','Q22', 'Q32', 'Q41', and 'Q51' are the angle coordinates of joints at the starting point. R1', 'R2', 'R3', 'R4' and 'R5' are the variables where the angle values of each

17 17 joint of the home position are stored. Home position is the starting point. After this store, it calls Koordinat subfunction. Koordinat: void koordinat() {cout<<"koordinatlari X,Y,Z sirasiyla giriniz:"; cin>>x2>>y>>z; cout<<"kartezyen koordinatlari girildi"<<endl; cout<<"aliÿma uzayi i inde olup olmadi i kontrol ediliyor."<<endl; sinirla(); Koordinat subfunction asks the Cartesian coordinates of the next destination. X2, Y, and Z which are the coordinates of the destination in cm. After entering these coordinates, it calls Sınırla subfunction in order to check this destination point whether it is reachable or not. Devamet: int devamet() {for(i=1;i<=5;i++) rtn[i]=rtn[i]+git[i]; cout<<"yeni konum girmek için 1e "<<endl; cout<<"başlangıç noktasına dönüp programdan çıkmak için 2ye basınız:"; cin>>cevap; Q11=T1; Q21=T2; Q31=T3; Q41=T4; Q51=T5; switch(cevap) {case 1: cout<<"yeni koordinatları giriniz:"; cin>>x2>>y>>z; sinirla();

18 18 break; case 2: cout<<"başlangıç noktasına dönüyorum."<<endl; for(i=1;i<=5;i++){a[i]=-(rtn[i]/(abs(rtn[i]))); bul[i]=abs(rtn[i]);; basla2(); break; ; return(0); This subfunction asks if the new position is going to be entered. If cevap variable is entered as 1, coordinates of new position are asked, else if cevap is entered as 2, program moves the robot to the home position. In addition the arrived position is registered as the starting point. The angle values of joints at this arrived position are stored in T1, T2, T3, T4 and T5 variables. Thus, these variables are registered in variables where the starting point is stored. If entering new position is selected, then the new position is asked by the program and then Sinirla subfunction is called. Else robot arm returns to the home position defined in the Referans subroutine. For this movement elements of array a are stored as 1 or -1. If element is smaller than zero, 1 is stored to this entry. Otherwise 1 is stored. Then Basla2 subfunction is called to return to the home position. Minimum: int minimum(int b[6]) { int k,i; k=0; for(i=1;i<=5;i++) k=(k+b[i]); for(i=1;i<=5;i++) { if((b[i]!=0)&&(k>b[i])) k=b[i];

19 19 return k; This subfunction finds the minimum element,which is different from zero, of the array that consists 5 positive number. At the first cycle in this subfunction, the variable k which shows the result of the subfunction is appointed as the sum of all elements. At the second cycle, elements are compared with this sum. If the element is different from zero and smaller than k, the value is appointed as k. This appointment is continued until the smallest value is found. This subfunction is needed for arranging the movement of motors together. This arrangement needs the number of pulses which is needed to run fist stopped motor. This number can be found with minimum subfunction. Sinyal: void sinyal(int c[6]) { if (((c[1]*a[1])==0)&&((c[2]*a[2])==0)) { for (i=1;i<=4;i++) porta[i]=0;; if (((c[1]*a[1])==0)&&((c[2]*a[2])>0)) {porta[1]=0x60;porta[2]=0xa0;porta[3]=0x90;porta[4]=0x50;; if (((c[1]*a[1])>0)&&((c[2]*a[2])==0)) {porta[1]=0x06;porta[2]=0x0a;porta[3]=0x09;porta[4]=0x05;; if (((c[1]*a[1])>0)&&((c[2]*a[2])>0)) {porta[1]=0x66;porta[2]=0xaa;porta[3]=0x99;porta[4]=0x55;; if (((c[1]*a[1])==0)&&((c[2]*a[2])<0)) {porta[1]=0x50;porta[2]=0x90;porta[3]=0xa0;porta[4]=0x60;; if (((c[1]*a[1])<0)&&((c[2]*a[2])==0)) {porta[1]=0x05;porta[2]=0x09;porta[3]=0x0a;porta[4]=0x06;; if (((c[1]*a[1])<0)&&((c[2]*a[2])<0)) {porta[1]=0x55;porta[2]=0x99;porta[3]=0xaa;porta[4]=0x66;;

20 20 if (((c[1]*a[1])<0)&&((c[2]*a[2])>0)) {porta[1]=0x65;porta[2]=0xa9;porta[3]=0x9a;porta[4]=0x56;; if (((c[1]*a[1])>0)&&((c[2]*a[2])<0)) {porta[1]=0x56;porta[2]=0x9a;porta[3]=0xa9;porta[4]=0x65;; if (((c[3]*a[3])==0)&&((c[4]*a[4])==0)) { for (i=1;i<=4;i++) portb[i]=0;; if (((c[3]*a[3])==0)&&((c[4]*a[4])>0)) {portb[1]=0x60;portb[2]=0xa0;portb[3]=0x90;portb[4]=0x50;; if (((c[3]*a[3])>0)&&((c[4]*a[4])==0)) {portb[1]=0x06;portb[2]=0x0a;portb[3]=0x09;portb[4]=0x05;; if (((c[3]*a[3])>0)&&((c[4]*a[4])>0)) {portb[1]=0x66;portb[2]=0xaa;portb[3]=0x99;portb[4]=0x55;; if (((c[3]*a[3])==0)&&((c[4]*a[4])<0)) {portb[1]=0x50;portb[2]=0x90;portb[3]=0xa0;portb[4]=0x60;; if (((c[3]*a[3])<0)&&((c[4]*a[4])==0)) {portb[1]=0x05;portb[2]=0x09;portb[3]=0x0a;portb[4]=0x06;; if (((c[3]*a[3])<0)&&((c[4]*a[4])<0)) {portb[1]=0x55;portb[2]=0x99;portb[3]=0xaa;portb[4]=0x66;; if (((c[3]*a[3])<0)&&((c[4]*a[4])>0)) {portb[1]=0x65;portb[2]=0xa9;portb[3]=0x9a;portb[4]=0x56;; if (((c[3]*a[3])>0)&&((c[4]*a[4])<0)) {portb[1]=0x56;portb[2]=0x9a;portb[3]=0xa9;portb[4]=0x65;; if((c[5]*a[5])>0) {portc[1]=0x05;portc[2]=0x09;portc[3]=0x0a;portc[4]=0x06;;

21 21 if((c[5]*a[5])<0) {portc[1]=0x06;portc[2]=0x0a;portc[3]=0x09;portc[4]=0x05;; if((c[5]*a[5])==0) {for(i=1;i<=4;i++) portc[i]=0;; Sinyal subfunction defines the 8-bit information which will be sent to a, b and c ports according to the information of motors direction. The four lowest significant bits of A(a3,a2,a1,a0) are reserved for the first motor and the four most significant bits of A(a7,a6,a5,a4) are reserved for the second motor. In this case the information that is sent to A port is not only depend on the direction of the first motor, but also direction of the second motor. At the first nine comparisons in the program the combinations for these two motors are considered and the 8-bit information is determined. The information which will be sent to B port is defined in the same way, because the 4 LSB of B(b3,b2,b1,b0) are reserved for motor3 and the 4 MSB of B(b7,b6,b5,b4) are reserved for motor4. The following nine comparisons are considered to solve this problem. The control of C port is a little different fom A and B ports. The 4 LSB of C port are reserved for motor5. However the four MSB of the C port are reserved for the DC motor which closes or opens the gripper. At the Sinyal subfunction only the signals which are sent to motor5 are calculated. The last three comparisons at the program are used to determine direction of motor5. The signals that must be sent to motors are listed below. Motor1: a3 a2 a1 a0 Hex Code Positive i-) ii-) a iii-) iv-)

22 22 Negative i-) ii-) iii-) a iv-) No direction Motor2: a7 a6 a5 a4 Hex Code Positive i-) ii-) a iii-) iv-) Negative i-) ii-) iii-) a iv-) No direction Motor3: b3 b2 b1 a0 Hex Code Positive i-) ii-) a iii-) iv-) Negative i-) ii-) iii-) a iv-) No direction

23 23 Motor4: b7 b6 b5 b4 Hex Code Positive i-) ii-) a iii-) iv-) Negative i-) ii-) iii-) a iv-) No direction Motor5: c3 c2 c1 a0 Hex Code Positive i-) ii-) iii-) a iv-) Negative i-) ii-) a iii-) iv-) No direction Başla2: void basla2(){ for (i=1;i<=5;i++) {bul1[i]=abs(bul[i]); if (bul[i]!=0) sign[i]=bul[i]/bul1[i];

24 24 else sign[i]=1; for (i=1;i<=5;i++){if (bul1[i]!=0) bul2[i]=bul1[i]-minimum(bul1); else bul2[i]=0;; for (i=1;i<=5;i++){if (bul1[i]!=0) bul1[i]=minimum(bul1); else bul1[i]=0;; for (i=1;i<=5;i++){if (bul2[i]!=0) bul3[i]=bul2[i]-minimum(bul2); else bul3[i]=0;; for (i=1;i<=5;i++){if (bul2[i]!=0) bul2[i]=minimum(bul2); else bul2[i]=0;; for (i=1;i<=5;i++){if (bul3[i]!=0) bul4[i]=bul3[i]-minimum(bul3); else bul4[i]=0;; for (i=1;i<=5;i++){if (bul3[i]!=0) bul3[i]=minimum(bul3); else bul3[i]=0;; for (i=1;i<=5;i++){if (bul4[i]!=0) bul5[i]=bul4[i]-minimum(bul4); else bul5[i]=0;; for (i=1;i<=5;i++){if (bul4[i]!=0) bul4[i]=minimum(bul4); else bul4[i]=0;; for (i=1;i<=5;i++){cout<<bul1[i]<<" "; for (i=1;i<=5;i++){cout<<bul2[i]<<" "; for (i=1;i<=5;i++){cout<<bul3[i]<<" "; for (i=1;i<=5;i++){cout<<bul4[i]<<" ";

25 25 for (i=1;i<=5;i++){cout<<bul5[i]<<" "; devir[1]=minimum(bul1); devir[2]=minimum(bul2); devir[3]=minimum(bul3); devir[4]=minimum(bul4); devir[5]=minimum(bul5); for(i=1;i<=5;i++)cout<<devir[i]<<" "; sinyal(bul1); cout<<porta[1]<<" "<<porta[2]<<" "<<porta[3]<<" "<<porta[4]<<endl; cout<<portb[1]<<" "<<portb[2]<<" "<<portb[3]<<" "<<portb[4]<<endl; cout<<portc[1]<<" "<<portc[2]<<" "<<portc[3]<<" "<<portc[4]<<endl; cout<<a[1]<<" "<<a[2]<<" "<<a[3]<<" "<<a[4]<<" "<<a[5]<<endl; for(i=1;i<=(4*devir[1]);i++) {m=i%4; if(m==0) m=4; outportb(0x2c0,porta[m]); outportb(0x2c1,portb[m]); outportb(0x2c2,portc[m]); delay(6); ; sinyal(bul2);

26 26 cout<<porta[1]<<" "<<porta[2]<<" "<<porta[3]<<" "<<porta[4]<<endl; cout<<portb[1]<<" "<<portb[2]<<" "<<portb[3]<<" "<<portb[4]<<endl; cout<<portc[1]<<" "<<portc[2]<<" "<<portc[3]<<" "<<portc[4]<<endl; cout<<a[1]<<" "<<a[2]<<" "<<a[3]<<" "<<a[4]<<" "<<a[5]<<endl; for(i=1;i<=(4*devir[2]);i++) {m=i%4; if (m==0) m=4; outportb(0x2c0,porta[m]); outportb(0x2c1,portb[m]); outportb(0x2c2,portc[m]); delay(6); ; sinyal(bul3); cout<<porta[1]<<" "<<porta[2]<<" "<<porta[3]<<" "<<porta[4]<<endl; cout<<portb[1]<<" "<<portb[2]<<" "<<portb[3]<<" "<<portb[4]<<endl; cout<<portc[1]<<" "<<portc[2]<<" "<<portc[3]<<" "<<portc[4]<<endl; cout<<a[1]<<" "<<a[2]<<" "<<a[3]<<" "<<a[4]<<" "<<a[5]<<endl; for(i=1;i<=(4*devir[3]);i++) {m=i%4; if (m==0) m=4; outportb(0x2c0,porta[m]); outportb(0x2c1,portb[m]); outportb(0x2c2,portc[m]); delay(6);;

27 27 sinyal(bul4); cout<<porta[1]<<" "<<porta[2]<<" "<<porta[3]<<" "<<porta[4]<<endl; cout<<portb[1]<<" "<<portb[2]<<" "<<portb[3]<<" "<<portb[4]<<endl; cout<<portc[1]<<" "<<portc[2]<<" "<<portc[3]<<" "<<portc[4]<<endl; cout<<a[1]<<" "<<a[2]<<" "<<a[3]<<" "<<a[4]<<" "<<a[5]<<endl; for(i=1;i<=(4*devir[4]);i++) {m=i%4; if (m==0) m=4; outportb(0x2c0,porta[m]); outportb(0x2c1,portb[m]); outportb(0x2c2,portc[m]); delay(6); ; sinyal(bul5); cout<<porta[1]<<" "<<porta[2]<<" "<<porta[3]<<" "<<porta[4]<<endl; cout<<portb[1]<<" "<<portb[2]<<" "<<portb[3]<<" "<<portb[4]<<endl; cout<<portc[1]<<" "<<portc[2]<<" "<<portc[3]<<" "<<portc[4]<<endl; cout<<a[1]<<" "<<a[2]<<" "<<a[3]<<" "<<a[4]<<" "<<a[5]<<endl; for(i=1;i<=(4*devir[5]);i++) {m=i%4; if (m==0) m=4; outportb(0x2c0,porta[m]); outportb(0x2c1,portb[m]); outportb(0x2c2,portc[m]);

28 28 delay(6); ; This subfunction calculates the number of signals to run the motors simultaneously. First, it takes the absolute values of the array Bul s elements and registers them to array Bul1. Then it defines the minimum element,which is different from zero, of the array Bul1 with using Minimum subfunction. Then it subtracts this minimum value from the elements which are bigger than zero and appoints the results to array Bul2. At the same time it appoints this minimum value to all members of Bul1 which are bigger than zero. At the next step Bul2 is taken as Bul1 and Bul3 is taken as bul2 and same calculations are done. These calculations continue until all the members of Bul5 are found. There is an example shown below for the calculations of the members of the Bul1, Bul2, Bul3, Bul4 and Bul5. Bul[1]=a+b Bul[2]=-(a+b+c) Bul[3]=-a Bul1[4]=a+b+c+d+e Bul1[5]=a+b+c+d ise Bul1[1]=a+b Bul1[2]=a+b+c Bul1[3]=a Bul1[4]=a+b+c+d+e Bul1[5]=a+b+c+d Bul1[1]=a Bul1[2]=a Bul1[3]=a Bul1[4]=a Bul1[5]=a Bul2[1]=b Bul2[2]=b Bul2[3]=0 Bul2[4]=b Bul2[5]=b Bul3[1]=0 Bul3[2]=c Bul3[3]=0 Bul3[4]=c Bul3[5]=c Bul4[1]=0 Bul4[2]=0 Bul4[3]=0 Bul4[4]=d Bul4[5]=d Bul5[1]=0 Bul5[2]=0 Bul5[3]=0 Bul5[4]=e Bul5[5]=0 Nth element of each array defines the number of pulses that are to be sent to the Nth motor.after calculating these elements Sinyal subfunction is executed for defining 8-bit information sent to A,B and C port according to elements of Bul1, Bul2, Bul3, Bul4 and Bul5 arrays and array a which includes direction informations. Finally defined 8-bit information are sent. This transmission is repeated depending on the members of bul arrays. Sınırla: void sinirla()

29 29 {if (Z<1 Z>102.7 Y>65) disarda(); else if ((X2>=60 && Z>40) (X2>60 && Z>=40)) disarda(); else if (X2>=57 && Z>=52) disarda(); else if (X2>=71 && Z>=25) disarda(); else if (((X2*X2)+(Z*Z))<850) disarda(); else if (((((Z-30.7)*(Z-30.7))+(X2*X2))*(0.707))>(65*65)) disarda(); else if (abs(y)>abs(x2)) disarda(); dongu1(); In this subfunction the entered x,y,z coordinates are tested whether they are reachable or not. If the coordinates are outside the region that the robot arm can reach, program goes to dısarda subfunction and asks us to enter new coordinates. Hesapla: void hesapla() { a[1]=q1-q11; while(a[1]>360.0) a[1]=a[1]-360.0; while(a[1]<(-360.0))a[1]=a[1]+360.0; if (a[1]<=360.0&&a[1]>=0.0) {if (a[1]>180.0) a[1]=a[1]-360.0;

30 30 if (a[1]<=180.0) a[1]=a[1]; ; if(a[1]>=-360.0&&a[1]<0.0) {if (a[1]>=-180.0) a[1]=a[1]; if (a[1]<-180.0) a[1]=360.0+a[1]; hesapla4(); This subfunction arranges the angle of first joint. While this angle is larger than then it subtracts 360 from this angle until it is smaller than While this angle is smaller than then it adds 360 to this angle until it is larger than These addition and substraction are needed for neglecting unnecessary return. After than it converts this to negative angle, if it is larger than and also it converts this angle to positive angle, if it is smaller than After these limitations it calls Hesapla4 subfunction. Yuvarla: int yuvarla(float r) { int b; b=r; if ((r-b)>=0.5) b++; return b; It converts real number to integer number. For conversion it appoints this real number to an integer value. Then it subtracts this integer value from real number. If the result is smaller then 0.5 the integer value is not changed. Otherwise it is increased by 1. Hesapla4: void hesapla4() { int vi,i,j; s[1]=abs( yuvarla((a[1]*4608.0)/360.0) ) ; bul[1]=s[1];

31 31 a[2]=q22-q21; s[2]=abs( yuvarla((a[2]*1800.0)/90.0) ) ; bul[2]=s[2]; a[3]=q32-q31; s[3]=abs( yuvarla((a[3]*2400.0)/80.0) ) ; bul[3]=s[3]; a[4]=q42-q41; s[4]=abs( yuvarla((a[4]*1350.0)/45.0) ) ; bul[4]=s[4]; if (count==1) a[5]=0-q51; else a[5]=q52-q51; s[5]=abs( yuvarla((a[5]*304.0)/180.0) ) ; bul[5]=s[5]; vi=0; cout<<"hesapla 4teyim"<<endl; for (i=1;i<=4;i++) for(j=(i+1);j<=5;j++) { if (bul[i]==bul[j]) vi++; if (vi==10) cout<<"zaten başlangictayiz."<<endl; cout<<"baslangıcta degilsek bu noktaya ulaşmak için yeni aci degerlerini giriniz."<<endl; if (a[1]<0) a[1]=-1; if (a[1]>=0) a[1]=1; if (a[5]<0) a[5]=-1; if (a[5]>=0) a[5]=1;

32 32 duzeltme(); The a[] arrays are the difference between the desired angle and the original angle in hesapla4 part. The required pulse to rotate the motors 1 o is known from the researches before. In this part to rotate the motors to desired angles pulse numbers are calculated and equaled to bul[] arrays. For instance, to rotate the first motor 360 o, the required pulse number is For second, third, fourth motors a[] arrays are the difference angles. Program calculates this difference angle for each motor and then converts it to pulse number that are going to be sent to motors like first motor calculations. Dongu1: void dongu1() {float Q2C,Q2CC,xa,za,t,Q2F,Q3F,Q4F; for(q2=(90);q2>=(5);q2=(q2-5)) { for(q3=0;q3<=110;q3=(q3+5)) {for(q4=0;q4<=35;q4=(q4+5)) {G1=90-Q2; G2=Q2+Q3; G3=G2+Q4; xa= *cos(G1*pi)+28.6*sin(G2*pi)+16.0*sin(G3*pi); za=(30.5+(28.0*sin(g1*pi))+(28.6*cos(g2*pi))+(16.0*cos(g3*pi))); X1=abs(X2)-xa; Z1=Z-za; t=(x1*x1)+(z1*z1); cout<<"t="<<t<<" "<<endl; if(((x1*x1)+(z1*z1))<10) break; cout<<"t="<<t<<" "<<endl;

33 33 if(((x1*x1)+(z1*z1))<10) break; cout<<"t="<<t<<" "<<endl; if(((x1*x1)+(z1*z1))<10) break; cout<<"xa="<<xa<<" "<<"za="<<za<<endl; if(q2==0) Q2F=0; else Q2F=2.5; if(q3==0) Q3F=0; else Q3F=2.5; if(q4==0) Q4F=0; else Q4F=2.5; cout<<q2f<<" "<<Q3F<<" "<<Q4F<<" "<<endl; for(q21=(q2-q2f);q21<=(q2+2.5);q21=(q21+0.5)) {for(q31=(q3-q3f);q31<=(q3+2.5);q31=(q31+0.5)) {for(q42=(q4-q4f);q42<=(q4+2.5);q42=(q42+0.5)) {G1=90-Q21; G2=Q21+Q31; G3=G2+Q42; xa= 3.0+(28.0*cos(G1*pi))+(28.6*sin(G2*pi))+(16.0*sin(G3*pi)); X1=abs(X2)-xa; za= (30.5+(28.0*sin(G1*pi))+(28.6*cos(G2*pi))+(16.0*cos(G3*pi))); Z1=Z-za; t=(x1*x1+z1*z1); cout<<"t="<<t<<" "<<endl; if(((x1*x1)+(z1*z1))<3) break; cout<<"t="<<t<<" "<<endl;

34 34 if(((x1*x1)+(z1*z1))<3) break; cout<<"t="<<t<<" "<<endl; if(((x1*x1)+(z1*z1))<3) break; Q21=Q21; Q31=Q31; Q42=Q42; for (Q1=0;Q1<=90;Q1=(Q1+1)) { ms=tan(q1*pi) ; msb=abs(x2)*ms ; Y1=abs(Y)-msb; if ((Y1*Y1)<1) break; cout<<"ms="<<ms<<" "<<"msb="<<msb<<endl; if ((X2<0)&&(Y<0)) Q1=180-Q1; else if ((X2<0)&&(Y>0)) Q1=180+Q1; else if ((X2>0)&&(Y>0)) Q1=360-Q1; cout<<"q1="<<q1<<" "<<"Q21="<<Q21<<" "<<"Q31="<<Q31<<" "<<"Q42="<<Q42<<endl; T1=Q1; T2=Q21; T3=Q31; T4=Q42; T5=Q51; Q52=Q51; cout<<"x1="<<x1<<" "<<"Y1="<<Y1<<" "<<"Z1="<<Z1<<endl; cout<<"polar koordinatlar hesaplandi"<<endl; cout<<"sinyal sayilari hesaplaniyor."<<endl; hesapla(); In dongu1 subfunction program searches that how can the robot arm reach the given x,y,z coordinates. Robot arm has three joints which are related with each other because of the

35 35 belts. Due to having only two equations and having three related joints, an exact solution can not be found. So, numerical methods are used to find the joint angles to reach the given x,y,z coordinates. In program three loop is used one within the other. The variable xa is the point that is gone while the program is in the loops. The variables X2, Y, Z are the coordinates which express the desired point. The variables X1 and Z1 are the difference between the desired point and the point where the robot arm goes in the loop. The X1 and Z1 can be expressed as the error. Inner loop is about the fourth motor. It is done between the angles 0 and 35. Second loop is about the motor 3 and it is done between the angles 0 and 110. outer loop is about the motor 2 and it is done between the angles 90 and 5. The calculations in these three loops are done to make the (X1*X1)+(Z1*Z1) smaller than 10. When the condition is provided, program goes to other loops which are one within the other, too. In these new loops, the lower limits and the upper limits are 2.5 o smaller and 2.5 o bigger than the found angles in earlier loops. The angles are increased by 0.5 o from the lower limit to the upper limit in the loops. When the condition (X1*X1)+(Z1*Z1)<3 is obtained, program exits from the loop. Program exits from the loop where the condition is first obtained, so the exact joint combination to reach the desired coordinates can not be found. This is due to the numerical solution. Duzeltme: void duzeltme() { cout<<"sinyal sayıları hesaplandı."<<endl; cout<<"sinyal sayıları üzerinde gerekli düzeltmeler yapılıyor."<<endl; if (a[5]<0) bul[5]=-1*bul[5]; cout<<"bul[5]="<<bul[5]<<endl; cout<<"a[2]="<<a[2]<<"a[3]="<<a[3]<<"a[4]="<<a[4]<<endl; if(a[2]>=0&&a[3]>=0&&a[4]>=0) duzeltme1();

36 36 else if(a[2]<=0&&a[3]<=0&&a[4]<=0) duzeltme3(); else if(a[2]<=0&&a[3]<=0&&a[4]>=0) duzeltme5(); else if(a[2]<=0&&a[3]>=0&&a[4]<=0) duzeltme9(); else if(a[2]<=0&&a[3]>=0&&a[4]>=0) duzeltme7(); else if(a[2]>=0&&a[3]<=0&&a[4]<=0) duzeltme6(); else if(a[2]>=0&&a[3]<=0&&a[4]>=0)duzeltme4(); else if(a[2]>=0&&a[3]>=0&&a[4]<=0) duzeltme2(); There are some relations among second, third and fourth joints of the topel robot because its movement is done with the belts. For example, if we move the second joint upward, the third joint moves downward although any pulse is not sent to the third motor. The compensation pulses are calculated to make the 3rd joint to stay on its original angle. At the same time, because the 3rd joint moves down, the 4th joint moves downward though any pulse is not sent to it. Of course, sometimes the third and fourth joints are wanted to be moved downward or upward so to move the joints to desired angles the compensation pulses which are calculated in duzeltme parts are added or subtracted to the pulses which are calculated in hesapla part. All the conditions of the joints are observed and different subprograma are written for those different conditions. The a[] arrays are the difference between the desired angle and the original angle. İn hesapla part. If all the second, third and fourth joints are wanted to be moved upward, program goes to the duzeltme1 subfunction. void duzeltme1() { cout<<"düzeltme 1deyim"<<endl; d32a=yuvarla(a[2]*710.0/90.0); cout<<"d32a="<<d32a<<endl; bul[3]=bul[3]+bul[2]+d32a;

37 37 cout<<"düzelme1 bul[3]="<<bul[3]<<endl; d42a=yuvarla(a[2]*140.0/90.0); cout<<"d42a="<<d42a<<endl; bul[4]=bul[4]+bul[3]-d42a; cout<<"düzelme1 bul[4].1="<<bul[4]<<endl; d43a=yuvarla(a[3]*50.0/210.0); cout<<"d43a="<<d43a<<endl; bul[4]=bul[4]-d43a; cout<<"düzelme1 bul[4].2="<<bul[4]<<endl; a[2]=1; a[3]=1; a[4]=1; d32a expresses the pulses which must be added to pulses which are sent to 3rd motor when the second motor is moved. bul[] arrays are the pulse numbers that are going to be sent motors. The variable bul[2] is the pulse number that is going to be sent to 2nd motor. When the bul[2] and d32a variables are added to bul[3] and appointed to bul[3], the 3th joint is made to g oto the desired angle. The variable d42a is the compensation pulse for fourth motor when the second motor is moved. When the calculated bul[4] pulses in hesapla part are added to, calculated bul[3] pulses in düzeltme part and then calculated d42a pulses are subtracted, the compensation of 4th motor according to 2nd motor is done. The variable d43a expresses the the compensation pulses for 4th motor when the 3rd is moved. When the variable d43a is subtracted from bul[4] which is calculated in düzeltme part, the pulses which are going to be sent to the 4th motor is determined. If a[2], a[3], a[4] arrays are 1 pulses which make the joints move upward are sent to motors. In this part all a[] arrays are 1 so joints move upward.

38 38 If the 2nd and 3rd joints are wanted to be moved upward, the 4th joint is wanted to moved downwarwd, program goes to duzeltme2 subfunction. void duzeltme2() { cout<<"düzeltme 2deyim"<<endl; d32a=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=bul[3]+bul[2]+d32a; cout<<"düzeltme2_2 bul[3]="<<bul[3]<<endl; d42a=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(-bul[4]+bul[3]-d42a); cout<<"düzelme2_2 bul[4].1="<<bul[4]<<endl; d43a=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]-d43a); cout<<"düzelme2_2 bul[4].2="<<bul[4]<<endl; if (((a[2]+a[3]))>abs(a[4])) a[4]=1; else a[4]=-1; a[2]=1; a[3]=1; If the variable bul[2] and d32a is added to bul[3] and then the sum is equaled to bul[3]. The 3rd joint is made to go the given angle. d42a expresses the compensation pulses that is arised from the wheel ratios between the 2nd and 4th motor. In the subfunction duzeltme2, the program is divided into two. If the arrays the sum of a[2] and a[3] is bigger than the absolute value of a[4], the pulses are sent to the 4th motor to move the 4th joint upward. Because the 2nd and 3rd joint are moved to up, the 4th joint has already moved downward. If the sum of a[2] and a[3] angles which are calculated in hesapla part is smaller than the absolute value of the angle the pulses are sent to 4th motor to move it downward. Because

39 39 though the 4th motor is moved to downward by the moving of other joints upward, it is not enough to move the 4th joint to the desired angle. In this duzeltme2 part the other joints are moved to upward. If all the second, third, and fourth joints are wanted to be moved to downward, program goes to the duzeltme3 subfunction. void duzeltme3() {cout<<"düzeltme 3deyim"<<endl; d32y=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; cout<<"düzeltme3 bul[3]="<<bul[3]<<endl; d42y=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=bul[4]+bul[3]-d42y; cout<<"düzelme3 bul[4].1="<<bul[4]<<endl; d43y=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=bul[4]-d43y; cout<<"düzelme3 bul[4].2="<<bul[4]<<endl; a[2]=-1; a[3]=-1; a[4]=-1; The calculations of bul[3] and bul[4] are seen above in program. The calculations and their explanations are like the explanations of duzeltme1. But here all a[] are -1, so the joints are moved to downward. If the joints 2 and 4 are required to be moved to upward, the joint 3 is moved to be downward, program goes to duzeltme4 subfunction. void duzeltme4()

40 40 {cout<<"düzeltme 4dayım"<<endl; if ((a[2])<=abs(a[3])) duzeltme4_2(); else{ d32y=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); cout<<"düzeltme4 d32y="<<d32y<<endl; cout<<"bul[2]="<<bul[2]<<endl; cout<<"düzeltme4 bul[3]="<<bul[3]<<endl; d42y=abs(yuvarla(a[2]*140.0/90.0)); cout<<"düzeltme4 d42y="<<d42y<<endl; bul[4]=abs(bul[4]+bul[3]-d42y); d43a=abs(yuvarla(a[3]*622.0/120.0)); bul[4]=abs(bul[4]-d43a); a[2]=1;a[3]=1; a[4]=1;; void duzeltme4_2() { cout<<"düzeltme 4_2dayım"<<endl; d32y=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); cout<<"düzeltme4_2 bul[3]="<<bul[3]<<endl; d42y=abs(yuvarla(a[2]*900.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); cout<<"düzeltme4_2 bul[4].1="<<bul[4]<<endl; d43a=abs(yuvarla(a[3]*622.0/120.0));

41 41 bul[4]=abs(bul[4]-d43a); cout<<"düzeltme4_2 bul[4].2="<<bul[4]<<endl; a[2]=1;a[3]=-1; a[4]=-1; In duzeltme4 part, program has two different paths. Here, as the 2nd joint is required to move upward, the 3rd joint is required to move downward. As it is explained before, the a[] values in hesapla part are the differences between the desired angles and the actual angle of that joint. If the variable a[2] is smaller than the variable a[3], program goes to duzeltme4_2 subfunction. This is reason is like the reason the of duzeltme2. While the direction of the 4th motor changes according to the difference angles in hesapla part, in part duzeltme4, the direction of the motor 3 changes according to those difference angles. In other compensation parts, the reason of the division of the program is same. Each compensation part is not going to be explained individually. The logic of all of them is same.

42 42 CONCLUSIONS AND SUGGESTIONS This project is designed for finding angles needed for entered Cartesian coordinates. However it can find wrong angles for some coordinates. The cause of this is there are three joints and two equations. The solution of finding joint angles to reach the desired coordinates can not be found exactly, it is found only with the numerical methods which is done in loops in the software. As explained before, while the program is calculating the required angles in loops, it can sometimes find wrong angles and these angles can not be in work space although program do not warn. Therefore; first of all, the points where the robot arm can reach exactly must be determined. In addition before the robot arm is run, the home position should be checked. Because if it is not at home position, robot arm can go to wrong points and the wheels and belts can be damaged. The software must be modified for fifth motor to run it successfully.

43 43 REFERENCES KORUL, Hakan. Robot Kollarının Video Kameradan Alınan Derinlik Bilgisi Geri Besleme ile Kontrolu, Yüksek Lisans Tezi, Osmangazi Üniversitesi, TÜRKAN, Mehmet Beş Eklemli Robot Kolu Kontrolu, Staj raporu, Osmangazi Üniversitesi, PCL-724 Interface card manual. GÖK, Abdurrahim. Modification of Robot Arms, Bitirme Tezi, Osmangazi Üniversitesi, CRAIG, J.J. Introduction to Robotics, Addison-Wesley Publishing Company, KLAFTER Richard D., CHMIELEWSKI Thomes A., NEGIN M. Robotics Engineering, Prentice-Hall International, Inc., 1989.

44 44 APPENDIX 1 THE SOFTWARE FOR CONTROLLING WITH COORDINATES #include <time.h> #include <process.h> #include <stdlib.h> #include <stdio.h> #include <dos.h> #include <conio.h> #include <iostream.h> #include <math.h> int sign[6],bul[6],devir[6],rtn[6],git[6],bul1[6],bul2[6],bul3[6],bul4[6],bul5[6],i,j,k,l,m,t,min; int porta[6],portb[6],portc[6]; void sinyal(int c[6]); int minimum(int b[6]); int basla1(); int basla(); void basla2(); /*60*/ int devamet(); /* aras*/ void donuyor(); /* aras*/ void koordinat(); /* aras*/ void sinirla(); /* aras*/ void disarda(); /* aras*/ void dongu1(); /* aras*/ void dongu2(); /* aras*/ void dongu3(); /* aras*/ void dongu4(); /* aras*/ void dongu5(); /* aras*/ void hesapla(); /* aras*/ void referans(); void hesapla4(); /* arasi*/ void duzeltme();/* arasi*/ void duzeltme1();/* arasi*/ void duzeltme2();/* arasi*/ void duzeltme3();/* arasi*/ void duzeltme4();/* arasi*/ void duzeltme5();/* arasi*/ void duzeltme6();/* arasi*/ void duzeltme7();/* arasi*/ void duzeltme8();/* arasi*/ void duzeltme9(); void duzeltme9_2();/* arasi*/ void duzeltme5_2(); void duzeltme7_2(); void duzeltme6_2(); void duzeltme4_2();

45 int yuvarla(float r); char harf,ac_kapa; float Q22,Q32,Q42,Q1,Q11,Q21,Q31,Q41,Q51,Q52,Q3,Q2,Q4,Q5,X1,X2,Y,Y1,Z,Z1,G1,G2,G3; float ms,msb,t1,t2,t3,t4,t5,r1,r2,r3,r4,r5; float pi=(m_pi/180.0); float a[6],a1[6]; int d32a,d42a,d43a,d32y,d42y,d43y; int count,s[6]; int cevap; int main() { for (i=1;i<=5;i++) rtn[i]=0; outportb(0x2c3,0x80); outportb(0x2c0,0x00); outportb(0x2c0,0x00); cout<<"basla"<<endl; count=1; while(count>=1) { cout<<"a aym m? Kapataym m?(a/k):"; cin>>ac_kapa; switch(ac_kapa){ case 'a': case 'A': outport(0x2c2,0xa0); delay(7000); break; case 'k': case 'K': outport(0x2c2,0x50); delay(7000); break; ; basla1(); /*if((count%2)==1){ outportb(0x2c2,0x50); delay(7000); ; if((count%2)==0){ outportb(0x2c2,0xa0); delay(7000);; */ cout<<"a aym m? Kapataym m?(a/k):"; cin>>ac_kapa; switch(ac_kapa){ case 'a': case 'A': outport(0x2c2,0xa0); delay(7000); break; case 'k': case 'K': outport(0x2c2,0x50); 45

46 46 delay(7000); break; ; if (cevap==2) break; count++;; return 0; int basla() { if (count>1) devamet(); else referans(); return(0); int devamet() { for(i=1;i<=5;i++) rtn[i]=rtn[i]+git[i]; for(i=1;i<=5;i++){cout<<"rtn["<<i<<"]="<<rtn[i]<<" ";; delay(1000); cout<<"yeni konum girmek i in 1e "<<endl; cout<<"baÿlang noktasna d np programdan kmak i in 2ye basnz:"; cin>>cevap; Q11=T1; Q22=T2; Q32=T3; Q41=T4; Q51=T5; switch(cevap) { case 1: cout<<"yeni koordinatlar giriniz:"; cin>>x2>>y>>z; sinirla(); break; case 2: cout<<"baÿlang noktasna d nyorum."<<endl; delay(3000); for(i=1;i<=5;i++){if (rtn[i]!=0) a[i]=-(rtn[i]/(abs(rtn[i]))); else a[i]=1; bul[i]=abs(rtn[i]);; for(i=1;i<=5;i++){cout<<"bul["<<i<<"]="<<bul[i]<<" ";; for(i=1;i<=5;i++){cout<<"a["<<i<<"]="<<a[i]<<" ";; delay(3000); basla2(); outportb(0x2c2,0xa0); delay(7000); exit(0); break; ; return(0);

47 47 void referans() { Q11=0; Q22=0; Q32=90.0; Q41=0; Q51=0; R1=Q11; R2=Q22; R3=Q32; R4=Q41; R5=Q51; koordinat(); void koordinat() { cout<<"koordinatlari X,Y,Z sirasiyla giriniz:"; cin>>x2>>y>>z; cout<<"kartezyen koordinatlari girildi"<<endl; cout<<" aliÿma uzayi i inde olup olmadi i kontrol ediliyor."<<endl; sinirla(); void sinirla() {if (Z<1 Z>102.7 Y>65) disarda(); else if ((X2>=60 && Z>40) (X2>60 && Z>=40)) disarda(); else if (X2>=57 && Z>=52) disarda(); else if (X2>=71 && Z>=25) disarda(); else if (((X2*X2)+(Z*Z))<850) disarda(); else if (((((Z-30.7)*(Z-30.7))+(X2*X2))*(0.707))>(65*65)) disarda(); else if (abs(y)>abs(x2)) disarda(); dongu1(); void disarda() { cout<<"koordinatlar calsma uzaynn dsnda"<<endl; cout<<"yeni koordinatlar giriniz"<<endl; koordinat(); cout<<"polar koordinatlar hesaplanmaya baÿland"<<endl; dongu1(); void dongu1() {float Q2C,Q2CC,xa,za,t,Q2F,Q3F,Q4F; for(q2=90;q2>=5;q2=(q2-5)) { for(q3=0;q3<=110;q3=(q3+5)) {for(q4=0;q4<=35;q4=(q4+5)) {G1=90.0-Q2; G2=Q2+Q3; G3=G2+Q4;

48 48 xa= *cos(G1*pi)+28.6*sin(G2*pi)+16.0*sin(G3*pi); za=(30.5+(28.0*sin(g1*pi))+(28.6*cos(g2*pi))+(16.0*cos(g3*pi))); X1=abs(X2)-xa; Z1=Z-za; t=(x1*x1)+(z1*z1); if(((x1*x1)+(z1*z1))<10) break; if(((x1*x1)+(z1*z1))<10) break; if(((x1*x1)+(z1*z1))<10) break; if(q2==0) Q2F=0; else Q2F=2.5; if(q3==0) Q3F=0; else Q3F=2.5; if(q4==0) Q4F=0; else Q4F=2.5; cout<<q2f<<" "<<Q3F<<" "<<Q4F<<" "<<endl; for(q21=(q2-q2f);q21<=(q2+2.5);q21=(q21+0.5)) {for(q31=(q3-q3f);q31<=(q3+2.5);q31=(q31+0.5)) {for(q42=(q4-q4f);q42<=(q4+2.5);q42=(q42+0.5)) {G1=90-Q21; G2=Q21+Q31; G3=G2+Q42; xa= 3.0+(28.0*cos(G1*pi))+(28.6*sin(G2*pi))+(16.0*sin(G3*pi)); X1=abs(X2)-xa; za= (30.5+(28.0*sin(G1*pi))+(28.6*cos(G2*pi))+(16.0*cos(G3*pi))); Z1=Z-za; t=(x1*x1+z1*z1); if(((x1*x1)+(z1*z1))<3) break; if(((x1*x1)+(z1*z1))<3) break; if(((x1*x1)+(z1*z1))<3) break; Q21=Q21; Q31=Q31; Q42=Q42; for (Q1=0;Q1<=90;Q1=(Q1+1)) { ms=tan(q1*pi) ; msb=abs(x2)*ms ; Y1=abs(Y)-msb; if ((Y1*Y1)<1) break;

49 if ((X2<0)&&(Y<0)) Q1=180-Q1; else if ((X2<0)&&(Y>0)) Q1=180+Q1; else if ((X2>0)&&(Y>0)) Q1=360-Q1; cout<<"q1="<<q1<<" "<<"Q21="<<Q21<<" "<<"Q31="<<Q31<<" "<<"Q42="<<Q42<<endl; T1=Q1; T2=Q21; T3=Q31; T4=Q42; T5=Q51; Q52=Q51; cout<<"x1="<<x1<<" "<<"Y1="<<Y1<<" "<<"Z1="<<Z1<<endl; cout<<"polar koordinatlar hesaplandi"<<endl; cout<<"sinyal sayilari hesaplaniyor."<<endl; hesapla(); void hesapla() { a[1]=q1-q11; while(a[1]>360.0) a[1]=a[1]-360.0; while(a[1]<(-360.0))a[1]=a[1]+360.0; if (a[1]<=360.0&&a[1]>=0.0) {if (a[1]>180.0) a[1]=a[1]-360.0; if (a[1]<=180.0) a[1]=a[1]; ; if(a[1]>=-360.0&&a[1]<0.0) {if (a[1]>=-180.0) a[1]=a[1]; if (a[1]<-180.0) a[1]=360.0+a[1]; hesapla4(); int yuvarla(float r) { int b; b=r; if ((r-b)>=0.5) b++; return b; void hesapla4() { int vi,i,j; s[1]=abs( yuvarla((a[1]*4608.0)/360.0) ) ; bul[1]=s[1]; a[2]=q22-q21; s[2]=abs( yuvarla((a[2]*1800.0)/90.0) ) ; bul[2]=s[2]; a[3]=q32-q31; s[3]=abs( yuvarla((a[3]*2400.0)/80.0) ) ; bul[3]=s[3]; a[4]=q42-q41; s[4]=abs( yuvarla((a[4]*1350.0)/45.0) ) ; bul[4]=s[4]; if (count==1) a[5]=0-q51; else a[5]=q52-q51; s[5]=abs( yuvarla((a[5]*304.0)/180.0) ) ; 49

50 50 bul[5]=s[5]; vi=0; cout<<"hesapla 4teyim"<<endl; cout<<"bul1="<<bul[1]<<" Bul2="<<bul[2]<<" Bul3="<<bul[3]<<" Bul4="<<bul[4]<<" Bul5="<<bul[5]<<endl; delay(1000); for (i=1;i<=4;i++) for(j=(i+1);j<=5;j++) { if (bul[i]==bul[j]) vi++; if (vi==10) cout<<"zaten baÿlangictayiz."<<endl; cout<<"baslangcta degilsek bu noktaya ulaÿmak i in yeni aci degerlerini giriniz."<<endl; if (a[1]<0) a[1]=-1; if (a[1]>=0) a[1]=1; if (a[5]<0) a[5]=-1; if (a[5]>=0) a[5]=1; duzeltme(); void duzeltme() { cout<<"sinyal saylar hesapland."<<endl; cout<<"sinyal saylar zerinde gerekli dzeltmeler yaplyor."<<endl; if (a[5]<0) bul[5]=-1*bul[5]; cout<<"bul[5]="<<bul[5]<<endl; cout<<"a[2]="<<a[2]<<"a[3]="<<a[3]<<"a[4]="<<a[4]<<endl; if(a[2]>=0&&a[3]>=0&&a[4]>=0) duzeltme1(); else if(a[2]<=0&&a[3]<=0&&a[4]<=0) duzeltme3(); else if(a[2]<=0&&a[3]<=0&&a[4]>=0) duzeltme5(); else if(a[2]<=0&&a[3]>=0&&a[4]<=0) duzeltme9(); else if(a[2]<=0&&a[3]>=0&&a[4]>=0) duzeltme7(); else if(a[2]>=0&&a[3]<=0&&a[4]<=0) duzeltme6(); else if(a[2]>=0&&a[3]<=0&&a[4]>=0)duzeltme4(); else if(a[2]>=0&&a[3]>=0&&a[4]<=0) duzeltme2(); void duzeltme1() { cout<<"dzeltme 1deyim"<<endl; d32a=yuvarla(a[2]*710.0/90.0); cout<<"d32a="<<d32a<<endl; bul[3]=bul[3]+bul[2]+d32a; cout<<"dzelme1 bul[3]="<<bul[3]<<endl; delay(1000); d42a=yuvarla(a[2]*140.0/90.0); cout<<"d42a="<<d42a<<endl; bul[4]=bul[4]+bul[3]-d42a; cout<<"dzelme1 bul[4].1="<<bul[4]<<endl; delay(1000); d43a=yuvarla(a[3]*50.0/210.0); cout<<"d43a="<<d43a<<endl; bul[4]=bul[4]-d43a;

51 51 cout<<"dzelme1 bul[4].2="<<bul[4]<<endl; delay(1000); a[2]=1; a[3]=1; a[4]=1; void duzeltme2() { cout<<"dzeltme 2deyim"<<endl; d32a=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=bul[3]+bul[2]+d32a; cout<<"dzeltme2_2 bul[3]="<<bul[3]<<endl; delay(1000); d42a=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(-bul[4]+bul[3]-d42a); cout<<"dzelme2_2 bul[4].1="<<bul[4]<<endl; delay(1000); d43a=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]-d43a); cout<<"dzelme2_2 bul[4].2="<<bul[4]<<endl; delay(1000); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43y ="<<d43y<<endl; if (((a[2]+a[3]))>abs(a[4])) a[4]=1; else a[4]=-1; a[2]=1; a[3]=1; void duzeltme3() { cout<<"dzeltme 3deyim"<<endl; d32y=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; d42y=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=bul[4]+bul[3]-d42y; d43y=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=bul[4]-d43y; a[2]=-1; a[3]=-1; a[4]=-1; void duzeltme4() { cout<<"dzeltme 4daym"<<endl; if ((a[2])<=abs(a[3])) duzeltme4_2(); else{ d32y=abs(yuvarla(a[2]*710.0/90.0));

52 52 bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(bul[4]+bul[3]-d42y); d43a=abs(yuvarla(a[3]*622.0/120.0)); bul[4]=abs(bul[4]-d43a); a[2]=1;a[3]=1; a[4]=1; void duzeltme4_2(){ cout<<"dzeltme 4_2daym"<<endl; d32y=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*900.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); d43a=abs(yuvarla(a[3]*622.0/120.0)); bul[4]=abs(bul[4]-d43a); a[2]=1;a[3]=-1; a[4]=-1; void duzeltme5() { cout<<"dzeltme 5deyim"<<endl; d32y=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; d42y=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(-bul[4]+bul[3]-d42y); d43y=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]-d43y); if ((abs(a[2]+a[3]))>a[4]) a[4]=-1; else a[4]=1; a[2]=-1; a[3]=-1; void duzeltme6() { cout<<"dzeltme 6daym"<<endl; cout<<" bul[2]="<<bul[2]<<endl; if (a[2]<=abs(a[3])) duzeltme6_2(); else { d32a=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32a); d42a=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(bul[4]+bul[3]+d42a); d43y=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]+d43y); a[3]=1; a[2]=1; a[4]=-1;;

53 53 void duzeltme6_2(){ cout<<"dzeltme 6_2daym"<<endl; d32y=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*800.0/90.0)); bul[4]=abs(bul[4]+bul[3]+d42y); d43a=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]+d43a); a[3]=-1; a[2]=1; a[4]=-1; ; void duzeltme7() { cout<<"dzeltme 7deyim"<<endl; if ((abs(a[2]))<=a[3]) duzeltme7_2(); else { d32y=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); d43a=abs(yuvarla(a[3]*622.0/120.0)); bul[4]=abs(bul[4]+d43a); a[3]=-1; a[2]=-1; a[4]=1; ; void duzeltme7_2(){ d32y=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*800.0/90.0)); bul[4]=abs(bul[4]+bul[3]+d42y); d43a=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]+d43a); a[3]=1; a[2]=-1; a[4]=1; ; void duzeltme9() { cout<<"dzeltme 9daym"<<endl; if (abs(a[2])<=a[3]) duzeltme9_2(); else{ d32y=abs(yuvarla(a[2]*710.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*140.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y);

54 54 d43a=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]+d43a); void duzeltme9_2(){ cout<<"dzeltme 9_2daym"<<endl; d32y=abs(yuvarla(a[2]*800.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*900.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); d43a=abs(yuvarla(a[3]*50.0/210.0)); bul[4]=abs(bul[4]+d43a); a[2]=-1;a[3]=1; a[4]=1; delay(1000); int minimum(int b[6]) { int k,i; k=0; for(i=1;i<=5;i++) k=(k+b[i]); for(i=1;i<=5;i++) { if((b[i]!=0)&&(k>b[i])) k=b[i]; return k; void sinyal(int c[6]) {if (((c[1]*a[1])==0)&&((c[2]*a[2])==0)) { for (i=1;i<=4;i++) porta[i]=0;; if (((c[1]*a[1])==0)&&((c[2]*a[2])>0)) {porta[1]=0x60;porta[2]=0xa0;porta[3]=0x90;porta[4]=0x50;; if (((c[1]*a[1])>0)&&((c[2]*a[2])==0)) {porta[1]=0x06;porta[2]=0x0a;porta[3]=0x09;porta[4]=0x05;; if (((c[1]*a[1])>0)&&((c[2]*a[2])>0)) {porta[1]=0x66;porta[2]=0xaa;porta[3]=0x99;porta[4]=0x55;; if (((c[1]*a[1])==0)&&((c[2]*a[2])<0)) {porta[1]=0x50;porta[2]=0x90;porta[3]=0xa0;porta[4]=0x60;; if (((c[1]*a[1])<0)&&((c[2]*a[2])==0)) {porta[1]=0x05;porta[2]=0x09;porta[3]=0x0a;porta[4]=0x06;; if (((c[1]*a[1])<0)&&((c[2]*a[2])<0)) {porta[1]=0x55;porta[2]=0x99;porta[3]=0xaa;porta[4]=0x66;; if (((c[1]*a[1])<0)&&((c[2]*a[2])>0)) {porta[1]=0x65;porta[2]=0xa9;porta[3]=0x9a;porta[4]=0x56;; if (((c[1]*a[1])>0)&&((c[2]*a[2])<0)) {porta[1]=0x56;porta[2]=0x9a;porta[3]=0xa9;porta[4]=0x65;; if (((c[3]*a[3])==0)&&((c[4]*a[4])==0)) { for (i=1;i<=4;i++) portb[i]=0;; if (((c[3]*a[3])==0)&&((c[4]*a[4])>0)) {portb[1]=0x60;portb[2]=0xa0;portb[3]=0x90;portb[4]=0x50;; if (((c[3]*a[3])>0)&&((c[4]*a[4])==0)) {portb[1]=0x06;portb[2]=0x0a;portb[3]=0x09;portb[4]=0x05;; if (((c[3]*a[3])>0)&&((c[4]*a[4])>0))