ROBOT KOLLARININ MODERNİZASYONU VE KONTROLÜ MUSTAFA SERKAN BOZKURT 151219992014 MEHMET FİDAN 151220002073 MEHMET KARADENİZ 151219982006 25.01.2004
İÇİNDEKİLER 1) GİRİŞ 2)GELİŞME 3)SONUÇ 4)KAYNAKÇA 5)EKLER
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.
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
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
MOTOR ÖZELLİKLER MOTOR GİRİŞİ Stepper 5 - Stepper 4 Stepper 3 Stepper 2 Stepper 1 Step Syn Stepping Motor Type I 03G770-2521I 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 03-820-024 I DC 4.5V 1.4A 2 DEG / STEP IBM P/N 2526734 LOT NO - SANYO CO..LTD. SANYO Step Syn Stepping Motor Type I 03-820-024 I IBM P/N 6838068 LOT NO - DC 4.5V 1.4A 2 DEG / STEP DENKI SANYO CO..LTD SANYO Step Syn Stepping Motor Type I 03-820-024 I IBM P/N 6838068 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.14 - - 4XSTEP SÜRÜCÜ Siyah pin.12 Yeşil pin.10 Turuncu pin.9 Beyaz pin.11 - - 4XSTEP SÜRÜCÜ İşaretli Beyaz pin.5 Beyaz pin.7 İşaretli Gri pin.8 Gri pin.6 - - 4XSTEP SÜRÜCÜ Kırmızı pin.4 Mavi pin.2 Siyah pin.1 Yeşil pin.3 - -
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ı
Ş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
Şekil-10 Topel Robot Eli Şekil-11 İç Anahtar Şekil-12 Dış Anahtar Şekil-13 PIO 82C55 Arayüz Kartı Şekil-14 82C55 Çıkış Terminali
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)
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;
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++)
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);
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(); /*70-170 aras*/ void donuyor(); /*100-140 aras*/ void koordinat(); /*180-230 aras*/ void sinirla(); /*240-340 aras*/ void disarda(); /*350-410 aras*/ void dongu1(); /*420-500 aras*/ void dongu2(); /*510-580 aras*/ void dongu3(); /*590-690 aras*/ void dongu4(); /*720-740 aras*/ void dongu5(); /*760-840 aras*/ void hesapla(); /*860-870 aras*/ void referans();
void hesapla4(); /*920-1070 arasi*/ void duzeltme();/*1100-1150 arasi*/ void duzeltme1();/*1150-1220 arasi*/ void duzeltme2();/*1230-1240 arasi*/ void duzeltme3();/*1240-1310 arasi*/ void duzeltme4();/*1320-1330 arasi*/ void duzeltme5();/*1330-1420 arasi*/ void duzeltme6();/*1430-1440 arasi*/ void duzeltme7();/*1440-1540 arasi*/ void duzeltme8();/*1550-1560 arasi*/ void duzeltme9();/*1560-1700 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;
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();
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= 3+ 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); /* 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)));
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()
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;
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;
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();
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;;
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); */
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;
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(); /*70-170 aras*/ void donuyor(); /*100-140 aras*/ void koordinat(); /*180-230 aras*/ void sinirla(); /*240-340 aras*/ void disarda(); /*350-410 aras*/ void dongu1(); /*420-500 aras*/ void dongu2(); /*510-580 aras*/ void dongu3(); /*590-690 aras*/ void dongu4(); /*720-740 aras*/ void dongu5(); /*760-840 aras*/ void hesapla(); /*860-870 aras*/ void hesapla2(); /*880-900 aras*/ void hesapla3(); /*910*/ void hesapla4(); /*920-1070 arasi*/
void duzeltme();/*1100-1150 arasi*/ void duzeltme1();/*1150-1220 arasi*/ void duzeltme2();/*1230-1240 arasi*/ void duzeltme3();/*1240-1310 arasi*/ void duzeltme4();/*1320-1330 arasi*/ void duzeltme5();/*1330-1420 arasi*/ void duzeltme6();/*1430-1440 arasi*/ void duzeltme7();/*1440-1540 arasi*/ void duzeltme8();/*1550-1560 arasi*/ void duzeltme9();/*1560-1700 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;;
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);
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); ;
/* 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;
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;
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()
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);
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))
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;;