ROBOT KOLLARININ MODERNİZASYONU VE KONTROLÜ

Transkript

1 ROBOT KOLLARININ MODERNİZASYONU VE KONTROLÜ MUSTAFA SERKAN BOZKURT MEHMET FİDAN MEHMET KARADENİZ

2 İÇİNDEKİLER 1) GİRİŞ 2)GELİŞME 3)SONUÇ 4)KAYNAKÇA 5)EKLER

3 GİRİŞ Bu projede 5 eklemli robot kolu incelendi ve geliştirildi. Topel robotumuz 4 step ve 1 DC motordan oluşmaktadır. Bu motorların özellikleri şekil 1de gösterilmektedir. Bu motorlar 2 sürücü kartıyla sürülmektedir. Bu sürücü kartlarından biri 4 step motoru, diğeri de 5. step motoru ve DC motoru sürüyor. Bu kartların devre şemaları şekil 2 ve 3 gösterilmektedir. DC motor robot elinin açılıp kapanmasında kullanılıyor. Robot eli içte ve dışta olmak üzere 2 anahtara sahip. İçteki anahtar el kapandıktan sonra motoru durduruyor. Dıştaki anahtar da el açıldıktan sonra motoru durduruyor. Bu anahtarlar sayesinde PC den DC motoru durdurmak için ayrıca bilgi gönderilmesi gerekmiyor. Bu anahtarların bağlantı şemaları şekil 4 te gösterilmektedir. Motorlardan step motor olan 4 tanesi robot kolun alt tarafında toplanmakta ve 1 step ve 1DC motor robotun uç tarafında toplanmaktadır. Altaki 4 step motor hareketlerini eklemlere kayışlar aracılığıyla iletmektedir. Uçtaki step ve DC motorlar hareketlerini güneş dişli sistemiyle iletmektedir. Uçtaki ve kayışların bağlı olduğu dişli sistemleri elektrik kesildiği anda eklemlerin bulunduğu konumda sabit kalmasını sağlamaktadır. 24 giriş-çıkış imkanı sağlayan ve 8255 programlanabilir arayüz çipinin 0 modunu kullanan bir PIO kartı kullanılmıştır. Bu 50 pin uyumlu I/O kartıyla PC den transistör ve optokuplerlerden oluşan devreye oradan da motorlara bilgi gönderilmektedir. Önceden de söylediğimiz gibi 2 tane sürücü kartı kullanılmıştır. Yani sadece 1 bilgisayar kullanılarak 2 tane bilgi yolu gerekmektedir. Bu yüzden, sürücü kartlarıyla PC arasında bir kart dizayn edilmiştir. Motorları sürmek için 28 bitlik bilgi gerekmesine rağmen tasarlanan I/O kartında 24 bitlik çıkış bulunmaktadır. Bu bir sorun oluşturmaktadır. Başka bir sorun ise PCL 724 PIO kartının A4 ve C4 pinlerinin çalışmamasıdır. Şekil 5te gösterilen 74HC04N inverterinin kullanıldığı bir devreyle sorun halledilmiştir. Yapılan bütün bağlantılar şekil 1de gösterilmektedir. GELİŞME Robot kol kinematiğinde verilen eklem açılarına göre pozisyon bulunur. Ters kinematikte ise girilen pozisyona ulaşabilmek için gerekli eklem açıları bulunur. Bu ters kinematik denklemlerinde birden fazla çözüm vardır. Bu çözümlerin sayısı robot koldaki eklem sayısına bağlı olmakla birlikte bundan başka bağlantı parametrelerine ve bunların ulaşabileceği alana da bağlıdır. Girilen nokta çalışma uzayı içindeyse en mantıklı olanı en yakın çözümü almaktır. Fakat yazılımda ters kinematiğin geometrik çözümü kullanılarak belli bir hata payı içerisine girildiğindeki ilk eklem açı değerleri alınmaktadır. Çözümler yapılırken Hakan Korul un yüksek lisans tezindeki denklemler kullanılmıştır. TOPEL robotunu çalıştıracak C programı yazılmıştır. Program ilk önce robot kolun bulunduğu başlangıç pozisyonunun eklemlerdeki açısal değerini referans fonksiyonunda istemektedir. Daha sonra gidilecek pozisyonun X, Y, Z koordinatlarını cm. cinsinden koordinat fonksiyonunda istenilmektedir. X, Y, Z olarak girilen pozisyonun kolun gidebileceği bir noktada olup olmadığını sınırla fonksiyonunda kontrol edilmektedir. Kontrol sonucu girilen pozisyon eğer çalışma uzayı dışarısındaysa program yeni konum girilmesini istemektedir. Eğer çalışma uzayı içerisindeyse döngü fonksiyonlarında gidilecek noktanın eklemlerdeki açısal değerleri hesaplanmaktadır. Bu hesaplanan açılar hesapla fonksiyonlarında motorlara gönderilecek sinyal sayılarına çevrilmektedir. TOPEL robotumuz kayışlarla hareket ettirildiğinden bir eklemin hareketi diğer eklemleri de etkilemektedir. Bunun sonucunda ortaya çıkan diğer eklemlerdeki düzeltme gereksinimi düzeltme fonksiyonlarında hesaplanıp motorlara gönderilecek sinyal sayılarına eklenmektedir.

4 Programda motorlara gönderilecek sinyal sayıları başla fonksiyonunda büyükten küçüğe sıralanmaktadır. Bu sıralamaya göre sinyal sayısı en küçük olan motorun hareketinin ilk önce, büyük olan motorun hareketinin de en sonda tamamlanacak şekilde sinyaller düzenlenmektedir. Düzenlenen bu sinyaller sinyal fonksiyonu kullanılarak motorları aynı anda çalıştıracak şekilde motorlara gönderilmektedir. Gidilecek noktaya gelindikten sonra yani eklemlerin hareketi durduktan sonra el eklemi bir nesneyi tutmak üzere kapanmaktadır. Nesne tutulduktan sonra devam et fonksiyonu kullanılarak ya nesnenin götürüleceği yeni koordinatların girilmesi istenilmekte ya da başlangıç noktasına dönülmektedir. Yeni konum girilirse nesne o konuma götürülüp bırakılmakta eğer başlangıç noktasına dönülürse nesne başlangıç noktasına bırakılıp programdan çıkılmaktadır. Programın akış şeması ekler bölümünde verilmiştir. SONUÇ TOPEL robot kolu yazılan C diliyle ister eklemlerin her birinin ayrı ayrı gideceği açılar verilerek ister kolun gideceği noktanın cm. cinsinden X, Y, Z konumları verilerek hareket ettirilmiştir. Ancak dişlilerdeki boşluklar yüzünden bazı eklemlerde sapmalar görülmüştür. Bu sapmalar girilen konumlara tam varılmasını engellemektedir. Yine de bu sapmalar önceden hesaba katılarak istenilen konumlara varılabilir. Eğer motorlar eklemler üzerine konulacak şekilde donanım tasarımı yapılırsa, eklemler birbirinden bağımsız hareket ettirilebilir ve yazılımdaki düzeltme fonksiyonlarının yazılmasına gerek kalmayacaktır. Böylelikle istenilen noktaya gitmek daha sağlıklı olacaktır. Aynı zamanda böyle bir donanım tasarımı sonucunda robotun çalışma uzayı da genişleyecektir. KAYNAKÇA KORUL, Hakan. Robot Kollarının Video Kameradan Alınan Derinlik Bilgisi Geri Besleme ile Kontrolu, bitirme tezi, 1995 TÜRKAN, Mehmet Beş Eklemli Robot Kolu Kontrolu, rapor, 2003 PIO-724 Arayüz kartı manuali

5 EKLER Programın Akış Şeması BAŞLA Başlangıç pozisyonunun açılarını gir hedef noktasını gir H çalışma uzayı içinde mi? E Açıları hesapla Sayıları hesapla Düzeltmeleri hesapla Motor datalarını bul Motorlara sinyalleri gönder el eklemini çalıştır devam edilecek mi? E H Başlangıç noktasına dön DUR ve ÇIK

6 MOTOR ÖZELLİKLER MOTOR GİRİŞİ Stepper 5 - Stepper 4 Stepper 3 Stepper 2 Stepper 1 Step Syn Stepping Motor Type I 03G I IBM PN 94X5937 INSUL. CLASS B DC 4.1V 1.1A 1.8 DEG / STEP LOT NO - SANYO DENKI CO..LTD. SANYO Step Syn Stepping Motor Type I I DC 4.5V 1.4A 2 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 DENKI Beyaz Mavi Sarı Kırmızı Mavi Kırmızı KırmBeyaz MaviBeyaz Siyah Beyaz Mavi Kımızı KırmBeyaz MaviBeyaz Siyah Beyaz Mavi Kırmızı KırmBeyaz MaviBeyaz Siyah Beyaz Mavi Kımızı KırmBeyaz MaviBeyaz Siyah Beyaz BAĞLANAN KABLO Kahverengi Blue Sarı Yeşil Mavi Turuncu Kahverengi Yeşil Gümüş Gümüş Mavi Turuncu Kahverengi Yeşil Gümüş Gümüş Mavi Turuncu Kaverengi Yeşil Gümüş Gümüş Mavi Orange Kahverengi Sarı Gümüş Gümüş SANYO DENKI CO..LTD şekil 1. (a) step motorlarının özellikleri ve bağlantıları KART ÇIKIŞI 1STEP+DC SÜRÜCÜ Kahverengi pin.7 Mavi pin.2 yellow pin.6 green pin.1 4XSTEP SÜRÜCÜ Beyaz pin.13 Turuncu pin.15 Yeşil pin.16 Mavi pin XSTEP SÜRÜCÜ Siyah pin.12 Yeşil pin.10 Turuncu pin.9 Beyaz pin XSTEP SÜRÜCÜ İşaretli Beyaz pin.5 Beyaz pin.7 İşaretli Gri pin.8 Gri pin XSTEP SÜRÜCÜ Kırmızı pin.4 Mavi pin.2 Siyah pin.1 Yeşil pin.3 - -

7 MOTOR End-effector* ( DC motor ) ÖZELLİKLER MOTOR GİRİŞİ BAĞLANAN KABLO KART ÇIKIŞI 1STEP+DC SÜRÜCÜ - Siyah Dış switch (II) - Beyaz Pembe Sarı pin.3 şekil 1. (b) DC motorun özellikleri ve bağlantıları Şekil-2 1x4 Step Motor Sürücü Kartı Şekil-3 1 step+dc Motor Sürücü Kartı Şekil-4 Anahtar Bağlantı Şeması Şekil-5 74HC04N Devre Şeması

8 Şekil-6 Yeni Robot Eli Şekil-7 Yeni Robot Elinin Step motora Bağlantısı Şekil-8 Elin DC motor-çark Bağlantısı Şekil-9 Elin Parmak Bağlantısı Şekil-10 Eli Kapatırken Durduracak Anahtar

9 Şekil-10 Topel Robot Eli Şekil-11 İç Anahtar Şekil-12 Dış Anahtar Şekil-13 PIO 82C55 Arayüz Kartı Şekil-14 82C55 Çıkış Terminali

10 ROBOTTEST.CPP /* Topel robotunun eklemlerini tek tek hareket ettiren program */ #include <time.h> #include <stdio.h> #include <dos.h> #include <conio.h> int Base=0x2c0; int d=5; void motor1_left(int step); void motor1_right(int step); void motor2_up(int step); void motor2_down(int step); void motor3_up(int step); void motor3_down(int step); void motor4_up(int step); void motor4_down(int step); void motor5_left(int step); void motor5_right(int step); void hand_open(); void hand_close(); int main(void) /*Base=0x378 - Control Register=Base+2 - Data Register=Base+0*/ int count; int i; int Vo; char a; /*Initialize*/ outportb(base+3,0x80); /*Set Port A as IN and Port B as OUT*/ outportb(base+0,0x00); outportb(base+1,0x00); outportb(base+2,0x00); while(!kbhit()) hand_close(); /* motor5_left(50);*/ void motor1_right(int step)

11 int i; for(i=0;i<step;i++) outportb(base+0,0x06); outportb(base+0,0x0a); outportb(base+0,0x09); outportb(base+0,0x05); void motor1_left(int step) int i; for(i=0;i<step;i++) outportb(base+0,0x05); outportb(base+0,0x09); outportb(base+0,0x0a); outportb(base+0,0x06); void motor2_up(int step) int i; for(i=0;i<step;i++) outportb(base+0,0x60); outportb(base+0,0xa0); outportb(base+0,0x90); outportb(base+0,0x50); void motor2_down(int step) int i;

12 for(i=0;i<step;i++) outportb(base+0,0x50); outportb(base+0,0x90); outportb(base+0,0xa0); outportb(base+0,0x60); void motor3_up(int step) int i; for(i=0;i<step;i++) outportb(base+1,0x06); outportb(base+1,0x0a); outportb(base+1,0x09); outportb(base+1,0x05); void motor3_down(int step) int i; for(i=0;i<step;i++) outportb(base+1,0x05); outportb(base+1,0x09); outportb(base+1,0x0a); outportb(base+1,0x06); void motor4_up(int step) int i; for(i=0;i<step;i++)

13 outportb(base+1,0x60); outportb(base+1,0xa0); outportb(base+1,0x90); outportb(base+1,0x50); void motor4_down(int step) int i; for(i=0;i<step;i++) outportb(base+1,0x50); outportb(base+1,0x90); outportb(base+1,0xa0); outportb(base+1,0x60); void motor5_left(int step) int i; for(i=0;i<step;i++) outportb(base+2,0x06); outportb(base+2,0x0a); outportb(base+2,0x09); outportb(base+2,0x05); void motor5_right(int step) int i; for(i=0;i<step;i++) outportb(base+2,0x05);

14 outportb(base+2,0x09); outportb(base+2,0x0a); outportb(base+2,0x06); void hand_open() outportb(base+2,0xa0); void hand_close() outportb(base+2,0x50); BASIC_C.CPP /*Topel robotu koordinatlar verilerek hareket ettiren program*/ #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],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(); /*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();

15 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();/* arasi*/ void sayac(); int yuvarla(float r); char harf; 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 T1,T2,T3,T4,T5,R1,R2,R3,R4,R5; float pi=(m_pi/180); float a[6],a1[6]; int d32a,d42a,d43a,d32y,d42y,d43y; int count,s[6]; int cevap; int main() outportb(0x2c3,0x80); outportb(0x2c0,0x00); outportb(0x2c0,0x00); cout<<"basla"<<endl; count=1; while(count>=1) basla1(); if((count%2)==1) outportb(0x2c2,0x50); delay(7000); ; if((count%2)==0) outportb(0x2c2,0xa0); delay(7000);; if (cevap==2) break; count++;; return 0; int basla() if (count>1) devamet(); else referans(); return(0); int devamet() cout<<"yeni konum girmek i in 1e "<<endl; cout<<"baÿlang noktasna d np programdan kmak i in 2ye basnz:"; cin>>cevap;

16 Q11=T1; Q21=T2; Q31=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; Q1=R1; Q22=R2; Q32=R3; Q42=R4; Q52=R5; hesapla(); break; ; return(0); void referans() cout<<"baÿlang noktasn Q11,Q21,Q31,Q41,Q51 olarak giriniz:" ; cin>>q11>>q21>>q31>>q41>>q51; R1=Q11; R2=Q21; R3=Q31; 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<3 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();

17 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,xa,za,t,Q2F,Q3F,Q4F; if (Z>65) Q2C=45; else Q2C=0; for(q2=(90-q2c);q2>=(50-q2c);q2=(q2-5)) for(q3=0;q3<=35;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); 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); /* delay(1000); */ if(((x1*x1)+(z1*z1))<10) break; if(((x1*x1)+(z1*z1))<10) break; if(((x1*x1)+(z1*z1))<10) break; cout<<"xa= "<<xa<<"za= "<<za<<endl; cout<<"q2="<<q2<<" Q3="<<Q3<<" Q4="<<Q4<<endl; cout<<"x1="<<x1<<" Z1="<<Z1<<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(q22=(q2-q2f);q22<=(q2+2.5);q22=(q22+0.5)) for(q32=(q3-q3f);q32<=(q3+2.5);q32=(q32+0.5)) for(q42=(q4-q4f);q42<=(q4+2.5);q42=(q42+0.5)) G1=90-Q22; G2=Q22+Q32; G3=G2+Q42; X1=abs(X2)-3-(28.0*cos(G1*pi))-(28.6*sin(G2*pi))-(16.0*sin(G3*pi)); Z1=Z-(30.5+(28.0*sin(G1*pi))+(28.6*cos(G2*pi))+(16.0*cos(G3*pi)));

18 t=(x1*x1+z1*z1); /* cout<<"t= "<<t<<endl; delay(1000);*/ if(((x1*x1)+(z1*z1))<3) break; if(((x1*x1)+(z1*z1))<3) break; if(((x1*x1)+(z1*z1))<3) break; /*Q22=Q2; Q32=Q3; Q42=Q4;*/ cout<<"q22="<<q22<<" Q32="<<Q32<<" Q42="<<Q42<<endl; cout<<"x1="<<x1<<" Z1="<<Z1<<endl; Q22=Q2; Q32=Q3; Q42=Q4; for (Q1=0;Q1<=90;Q1=(Q1+2)) Y1=abs(Y)-abs(X2)*tan(Q1*pi); if ((Y1*Y1)<1) break; 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<<" "<<"Q22="<<Q22<<" "<<"Q32="<<Q32<<" "<<"Q42="<<Q42<<endl; T1=Q1; T2=Q22; T3=Q32; 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()

19 int vi,i,j; s[1]=abs( yuvarla((a[1]*4960.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]*4150.0)/90.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]*148.0)/180.0) ) ; bul[5]=s[5]; vi=0; cout<<"bul1="<<bul[1]<<" Bul2="<<bul[2]<<" Bul3="<<bul[3]<<" Bul4="<<bul[4]<<" Bul5="<<bul[5]<<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<<"baslangcta degilsek bu noktaya ulaÿmak i in yeni aci degerlerini giriniz."<<endl; 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 duzeltme2(); void duzeltme1() cout<<"dzeltme 1deyim"<<endl; d32a=yuvarla(a[2]*1420.0/90.0); cout<<"d32a="<<d32a<<endl; bul[3]=bul[3]+bul[2]+d32a; cout<<"bul[3]="<<bul[3]<<endl; d42a=yuvarla(a[2]*280.0/90.0); cout<<"d42a="<<d42a<<endl; bul[4]=bul[4]+bul[3]-d42a; cout<<"bul[4]="<<bul[4]<<endl; d43a=yuvarla(a[3]*1244.0/120.0); cout<<"d43a="<<d43a<<endl; bul[4]=bul[4]-d43a; cout<<"bul[4]="<<bul[4]<<endl; a[2]=1;

20 a[3]=1; a[4]=1; // sayac(); void duzeltme2() cout<<"dzeltme 2deyim"<<endl; if (a[2]<=0&&a[3]<=0&&a[4]<=0) duzeltme3(); else duzeltme4(); void duzeltme3() cout<<"dzeltme 3deyim"<<endl; d32y=abs(yuvarla(a[2]*1420.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; d42y=abs(yuvarla(a[2]*280.0/90.0)); bul[4]=bul[4]+bul[3]-d42y; cout<<"bul[4]="<<bul[4]<<endl; d43y=abs(yuvarla(a[3]*1244.0/120.0)); bul[4]=bul[4]-d43y; cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43y ="<<d43y<<endl; a[2]=-1; a[3]=-1; a[4]=-1; // sayac(); // void duzeltme4() cout<<"dzeltme 4teyim"<<endl; if (a[2]<=0&&a[3]<=0&&a[4]>=0) duzeltme5(); else duzeltme6(); void duzeltme5() cout<<"dzeltme 5deyim"<<endl; d32y=abs(yuvarla(a[2]*1420.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; d42y=abs(yuvarla(a[2]*280.0/90.0)); bul[4]=abs(-bul[4]+bul[3]-d42y); cout<<"bul[4]="<<bul[4]<<endl; d43y=abs(yuvarla(a[3]*1244.0/120.0)); bul[4]=abs(bul[4]-d43y); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43y ="<<d43y<<endl; a[2]=-1; a[3]=-1;

21 a[4]=1; // sayac(); void duzeltme6() cout<<"dzeltme 6daym"<<endl; if (a[2]<=0&&a[3]>=0&&a[4]>=0) duzeltme7(); else duzeltme8(); void duzeltme7() cout<<"dzeltme 7deyim"<<endl; d32y=abs(yuvarla(a[2]*1420.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*280.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); cout<<"bul[4]="<<bul[4]<<endl; d43a=abs(yuvarla(a[3]*1244.0/120.0)); bul[4]=abs(bul[4]-d43a); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43a ="<<d43a<<endl; a[2]=-1; a[3]=1; a[4]=1; // sayac(); void duzeltme8() cout<<"dzeltme 8deyim"<<endl; if (a[2]<=0&&a[3]>=0&&a[4]<=0) duzeltme9(); // else sayac(); void duzeltme9() cout<<"dzeltme 9daym"<<endl; d32y=abs(yuvarla(a[2]*1420.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*280.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); cout<<"bul[4]="<<bul[4]<<endl; d43a=abs(yuvarla(a[3]*1244.0/120.0)); bul[4]=abs(bul[4]+d43a); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43a ="<<d43a<<endl; a[2]=-1; a[3]=1; a[4]=-1; cout<<"dzeltmeler bitti."<<endl; cout<<"motorlara gidecek gerekli sinyaller oluÿturuluyor."<<endl; cout<<"sinyaller g nderiliyor ve saya kontrol ediliyor."<<endl; delay(5000); // sayac();

22 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)) 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;;

23 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;; int basla1() basla(); for (i=1;i<=5;i++) bul1[i]=abs(bul[i]); if (bul[i]!=0) sign[i]=bul[i]/bul1[i]; 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]<<" "; 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]<<" "; /* delay(15000); */

24 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); ; /* delay(15000); */ sinyal(bul2); 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); ; /* delay(15000); */ 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); ; /* delay(15000); */ 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;

25 if (m==0) m=4; outportb(0x2c0,porta[m]); outportb(0x2c1,portb[m]); outportb(0x2c2,portc[m]); delay(6); ; /* delay(15000);*/ 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]); delay(6); ; /* delay(15000);*/ return(0); TEST2.CPP /* Topel robotu eklem açıları verilerek hareket ettiren program */ #include <time.h> #include <stdio.h> #include <dos.h> #include <conio.h> #include <iostream.h> #include <math.h> int sign[6],bul[6],devir[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]); void basla(); /*60*/ void 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 hesapla2(); /* aras*/ void hesapla3(); /*910*/ void hesapla4(); /* arasi*/

26 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();/* arasi*/ void sayac(); int yuvarla(float r); char harf; float Q22,Q32,Q42,Q1,Q11,Q21,Q31,Q41,Q51,Q3,Q2,Q4,Q5,X1,X2,Y,Y1,Z,Z1,G1,G2,G3; float pi=(m_pi/180); float R1,R2,R3,R4,R5; float a[6],a1[6]; int d32a,d42a,d43a,d32y,d42y,d43y; int count,s[6]; int cevap; int main() cout<<"q1 Q11 Q22 Q21 Q32 Q31 Q42 Q41 Q51 giriniz"<<endl; cin>>q1>>q11>>q22>>q21>>q32>>q31>>q42>>q41>>q51; R1=Q11; R2=Q21; R3=Q31; R4=Q41; R5=Q51; cout<<"q1="<<q1<<"q11="<<q11<<"q22="<<q22<<"q21="<<q21<<"q32="<<q32<<" Q31="<<Q31<<"Q42="<<Q42<<"Q41="<<Q41<<"Q51="<<Q51<<endl; count=1; basla(); while(count>=1) if ((count%2)==1) outportb(0x2c2,0x50); delay(7000); elseoutportb(0x2c2,0xa0); delay(7000); count++; cout<<"yeni konum girmek i in 1e "<<endl; cout<<"baÿlang noktasna d np programdan kmak i in 2ye basnz:"; cin>>cevap; Q11=Q1; Q21=Q22; Q31=Q32; Q41=Q42; Q51=Q51; switch(cevap) case 1: cout<<"yeni koordinatlar giriniz:"; cin>>q1>>q22>>q32>>q42>>q51; basla(); break; case 2: cout<<"baÿlang noktasna d nyorum."<<endl; Q1=R1; Q22=R2; Q32=R3; Q42=R4; Q51=R5; basla(); break;; if (cevap==2) break;;

27 return 0; void basla() 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]; else a[1]=360.0+a[1]; hesapla4(); outportb(0x2c3,0x80); outportb(0x2c0,0x00); outportb(0x2c0,0x00); for (i=1;i<=5;i++) bul1[i]=abs(bul[i]); if (bul[i]!=0) sign[i]=bul[i]/bul1[i]; 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);

28 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]<<" "; 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]<<" "; /* delay(15000); */ 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; 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); ; /* delay(15000); */ sinyal(bul2); 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; 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); ;

29 /* delay(15000); */ 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; 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); ; /* delay(15000); */ 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; 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); ; /* delay(15000);*/ 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; 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]); delay(6); ; int yuvarla(float r) int b; b=r; if ((r-b)>=0.5) b++; return b; void hesapla4() int vi,i,j;

30 s[1]=abs( yuvarla((a[1]*4960.0)/360.0) ) ; bul[1]=s[1]; cout<<"bul[1]="<<bul[1]<<endl; a[2]=q22-q21; cout<<"a[2]="<<a[2]<<endl; s[2]=abs( yuvarla((a[2]*1800.0)/90.0) ) ; bul[2]=s[2]; cout<<"bul[2]="<<bul[2]<<endl; a[3]=q32-q31; cout<<"a[3]="<<a[3]<<endl; s[3]=abs( yuvarla((a[3]*4150.0)/90.0) ) ; bul[3]=s[3]; cout<<"bul[3]="<<bul[3]<<endl; a[4]=q42-q41; cout<<"a[4]="<<a[4]<<endl; s[4]=abs( yuvarla((a[4]*1350.0)/45.0) ) ; bul[4]=s[4]; cout<<"bul[4]="<<bul[4]<<endl; a[5]=0-q51; cout<<"a[5]="<<a[5]<<endl; s[5]=abs( yuvarla((a[5]*148.0)/180.0) ) ; bul[5]=s[5]; cout<<"bul[5]="<<bul[5]<<endl; vi=0; cout<<"bul1="<<bul[1]<<" Bul2="<<bul[2]<<" Bul3="<<bul[3]<<" Bul4="<<bul[4]<<" Bul5="<<bul[5]<<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<<"baslangcta degilsek bu noktaya ulaÿmak i in yeni aci degerlerini giriniz."<<endl; 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 duzeltme2(); void duzeltme1() cout<<"dzeltme 1deyim"<<endl;

31 d32a=yuvarla(a[2]*2550.0/90.0); cout<<"d32a="<<d32a<<endl; bul[3]=bul[3]+bul[2]+d32a; cout<<"bul[3]="<<bul[3]<<endl; d42a=yuvarla(a[2]*230.0/90.0); cout<<"d42a="<<d42a<<endl; bul[4]=bul[4]+bul[3]-d42a; cout<<"bul[4]="<<bul[4]<<endl; d43a=yuvarla(a[3]*800.0/120.0); cout<<"d43a="<<d43a<<endl; bul[4]=bul[4]-d43a; cout<<"bul[4]="<<bul[4]<<endl; a[2]=1; a[3]=1; a[4]=1; // sayac(); void duzeltme2() cout<<"dzeltme 2deyim"<<endl; if (a[2]<=0&&a[3]<=0&&a[4]<=0) duzeltme3(); else duzeltme4(); void duzeltme3() cout<<"dzeltme 3deyim"<<endl; d32y=abs(yuvarla(a[2]*2550.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; d42y=abs(yuvarla(a[2]*230.0/90.0)); bul[4]=bul[4]+bul[3]-d42y; cout<<"bul[4]="<<bul[4]<<endl; d43y=abs(yuvarla(a[3]*800.0/120.0)); bul[4]=bul[4]-d43y; cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43y ="<<d43y<<endl; a[2]=-1; a[3]=-1; a[4]=-1; // sayac(); // void duzeltme4() cout<<"dzeltme 4teyim"<<endl; if (a[2]<=0&&a[3]<=0&&a[4]>=0) duzeltme5(); else duzeltme6(); void duzeltme5()

32 cout<<"dzeltme 5deyim"<<endl; d32y=abs(yuvarla(a[2]*2550.0/90.0)); bul[3]=bul[3]+bul[2]+d32y; d42y=abs(yuvarla(a[2]*230.0/90.0)); bul[4]=abs(-bul[4]+bul[3]-d42y); cout<<"bul[4]="<<bul[4]<<endl; d43y=abs(yuvarla(a[3]*800.0/120.0)); bul[4]=abs(bul[4]-d43y); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43y ="<<d43y<<endl; a[2]=-1; a[3]=-1; a[4]=1; // sayac(); void duzeltme6() cout<<"dzeltme 6daym"<<endl; if (a[2]<=0&&a[3]>=0&&a[4]>=0) duzeltme7(); else duzeltme8(); void duzeltme7() cout<<"dzeltme 7deyim"<<endl; d32y=abs(yuvarla(a[2]*2550.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*230.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y); cout<<"bul[4]="<<bul[4]<<endl; d43a=abs(yuvarla(a[3]*800.0/120.0)); bul[4]=abs(bul[4]-d43a); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43a ="<<d43a<<endl; a[2]=-1; a[3]=1; a[4]=1; // sayac(); void duzeltme8() cout<<"dzeltme 8deyim"<<endl; if (a[2]<=0&&a[3]>=0&&a[4]<=0) duzeltme9(); // else sayac(); void duzeltme9() cout<<"dzeltme 9daym"<<endl; d32y=abs(yuvarla(a[2]*2550.0/90.0)); bul[3]=abs(-bul[3]+bul[2]+d32y); d42y=abs(yuvarla(a[2]*230.0/90.0)); bul[4]=abs(-bul[4]+bul[3]+d42y);

33 cout<<"bul[4]="<<bul[4]<<endl; d43a=abs(yuvarla(a[3]*800.0/120.0)); bul[4]=abs(bul[4]+d43a); cout<<"d32y="<<d32y<<"bul[3]="<<bul[3]<<"d42y="<<d42y<<"bul[4]="<<bul[4]<<"d43a ="<<d43a<<endl; a[2]=-1; a[3]=1; a[4]=-1; cout<<"dzeltmeler bitti."<<endl; cout<<"motorlara gidecek gerekli sinyaller oluÿturuluyor."<<endl; cout<<"sinyaller g nderiliyor ve saya kontrol ediliyor."<<endl; delay(5000); // sayac(); 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))

34 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;; 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;;