Paralel Robot Elektronik Devre Step Motor devre 4 kattan oluşur. İlk kat LCD ekranın bulunduğu kattır. LCD ekranda robotun xyz konumu, açı bilgisi ve step motor adım bilgisi görüntülenebilmektedir. İkinci kat ana pic in (16f4620)bulunduğu kısımdır bu katta ayrıca pic e max232 entegre bağlantısı yapılmıştır. Max232 seri port ile pic arasındaki voltaj farklılığını ayarlayıp pic ile bilgisayar arasındaki bağlantıyı kurar.
Pic ler 5v ile haberleşir bilgisayarlar ise +12v ile haberleşir arada bu voltaj farklılığını ayarlayan bir entegre konulmaz ise bilgisayardan gelen 12v luk sinyal pic in yanması için yeterlidir. Üçüncü katta yönvericiler ve step motorların istenilen adımı atmasını sağlayan çocuk olarak adlandırabileceğimiz 16f628 model picler bulunur. Her bir motor için 1 adet 16f628 vardır çünkü motorların eş zamanlı hareket etmesi gerekmektedir. Dödrdüncü katta ise transistörler bulunmaktadır.
Bu transistörler piclerden gelen 5 voltluk sinyali yükselterek 12 voltluk step motorları çalıştırmaktadır. Bunlara ek olarak buton katı bulunmaktadır ve ana picin bulunduğu ikinci kata jak ile bağlantısı yapılmaktadır. Buton katında robotu xyz eksenlerinde ileri geri yönlendirmeye yarayan butonlar bulunmaktadır. +12v luk besleme gerilimi ikinci kata jak ile takılır ve 7805 ile +5v a regüle edilerek piclere, yön vericilere, lcd ekrana ve max232 entegrelerine gönderilir.
Paralel Robot Elektronik Devre Şeması
Baskı devre şemaları
Devre şemaları ve baskı devreler proteus programında çizilmiştir. Sadece son kat daha önceden başka bir proje için yapıldığından expresspcb programında çizilmiş ve sonradan bu porjeye entegre edilmiştir.
LCD ekranın ve ekran değiştirmeye yarayan butonların bulunduğu 1.kat
Paralel Robot LCD Ekran Katı Baskı Devresi
Ana pic in bulunduğu 2. Katın baskı devresi. Bu katta max232 entegresi ile seri iletişim portunun pic e bağlantısı yapılmış olup pic içinde yazılan kodda bilgisyar iletişimi yazılmamıştır. Daha sonraki tahrikli mekanizmalarda bilgisyar iletişim bağlantısını destekleyen kod eklenmiştir.
Paralel Robot Haberleşme ve PIC Katı Baskı Devresi
Yönlendiriciler ve çocuk pic lerin bulunduğu kat. Çift taraflı plakete yapılmıştır.
Paralel Robot Motor PIC Katı Baskı Devresi
Delta robotu xyz eksenlerinde yönlendirmeye yarayan buton gruplarının bulunduğu buton katı. Soldaki butonlar boşta kalan sonradan fonksiyon atanması düşünülen butonlardır.
Paralel Robot Buton Katı Baskı Devresi
4.kat motor sürücü transistör katıdır. Bu katta pic lerden gelen 5v luk sinyal 12 volta yükseltilerek step motorların sürülmesi sağlanır. Step motorlardan 5 kablo çıkar, 4 tanesi sırası önem teşkil etmekle birlikte klamenslere bağlanır. 5. Kablo +12v a bağlantısı yapılır.
Paralel Robot Step Motor Sürücü Baskı Devresi
Paralel Robot Elektronik Pic programı ve Algoritma
Pic programı C dilinde CCS C derleyicisi kullanılarak yazılmış ve derlenmiştir. İki pic bulunduğundan iki farklı pic programı bulunmaktadır. Biri ana picin programı xyz_kontrol_18F4620.c, diğeride çocuk picin programıdır xyz_kontrol_16f628a.c.
Sisteme enerji verildiğinde lcd ekranda ilk olarak 3sn boyunca giriş ekranı görüntülenir. Ardından Position” ekranında robotun o anki pozisonu görüntülenir. Lcd butonuna ardarda basılarak “Angle”, “Step Count” ve diğer ekranlar görüntülenebilir. Robotun ilk kurulumdan sonra kalibrasyonundan ayarı yapılabilmesi için “Kalibre” butonu bir kez kontrol edilir. Ardından eksen butonları için tarama kısır döngüye girer. Bu esnada eksen butonlarından girilen koordinat bilgisinin açı değerleri robotun sıfır posizyonuna göre hesaplanır. Bu değerlere göre step motorun hareketi için adım değerleri hesaplanır.
Adım değerleri baz alınarak üç motorunda aynı zamanda çalışmaya başlayıp sonlanması için adımlar arası zaman değişkenleri hesaplanıp motor adım bilgisi ile motor sürücü microdenetleyicilerine (PIC16F628A, xyz_kontrol_16F628A.C) gönderilir. Motor sürücü microdenetleyicileri tarafından alınan bu değerler pozitif ise motoru o değer kadar ve hesaplanan gecikme süreleri ile motora hareket verir. Değer negatif ise aynı şekilde ters yönde hereket verir. Eğer kumandadan girilen koordinat bilgisi robotun çalışma uzayı dışında ise lcd ekranda hata mesajı görütülenir ve kullanıcıdan çalışma uzayı içinde bir nokta girilmesi beklenir.
Ana pic programı
///////////////////////////////////////////////////////////////////////////////
//// xyz_kontrol_18F4620.C ////
//// ////
//// Bu program 3-dof delata robotu, x,y,z eksenleri doğrultusunda ////
//// kullanıcı tarafından 10 micron ile 10 cm hassasiyetle kumandadan ////
//// kontrol edilmesini sağlar. ////
//// ////
//// ////
//// Robot geometry: ////
//// e = 109.12; // end effector ////
//// f = 346.41; // base ////
//// re = 323.59; // rod ////
//// rf = 133.09; // arm ////
//// ////
//// Kullanılan kontrolor 18F46K20 (32Kb, 4MHz), 16F628A(2Kb, 4MHz) ////
//// LCD ekran HD44780 tabalı 4x16 HY-1604A06 alphanumeric numeric ////
//// ////
//// LCD ekran pinleri: PORTB ////
//// Vss 1: GND Data bağlantıları ////
//// Vdd 2: +5V ////
//// Vee 3: 10k POT ////
//// RS 4: RB1 ////
//// RW 5: RB2 ////
//// E 6: RB0 ////
//// D0-D3 7-10: No connection ////
//// D4 11: RB4 ////
//// D5 12: RB5 ////
//// D6 13: RB6 ////
//// D7 14: RB7 ////
//// ////
//// XYZ eksen butonları: PORTD ////
//// +X: RD0 ////
//// -X: RD1 ////
//// +Y: RD2 ////
//// -Y: RD3 ////
//// +Z: RD4 ////
//// -Z: RD5 ////
//// ////
//// Diğer butonlar: PORTD ////
//// LCD : RD6 ////
//// Hassasiyet : RD7 ////
//// : RC2 ////
//// : RC3 ////
//// : RC4 ////
//// : RC5 ////
//// ////
///////////////////////////////////////////////////////////////////////////////
//// YAZAN: Remzi ŞAHİNOĞLU ////
//// TARİH: 24 Mayıs 2011 ////
//// ////
///////////////////////////////////////////////////////////////////////////////
#if defined(__PCH__)
#include <18F4620.h>
#include <MATH.h>
#use delay(clock=4000000)
#fuses XT, NOWDT, NOPROTECT, NOLVP, NOBROWNOUT, NOPUT, NOWRT, NODEBUG, NOCPD
#use rs232 (baud=9600, xmit=PIN_C6, rcv=PIN_c7, parity=N, stop=1)
#include <stdlib.h> // bu sıralama dogru rs232 den altta olmalı
#include <input.c>
#define use_portb_lcd TRUE // lcd bilgisi için port b’yi kullanıyoruz
#include <LCD416.c> // lcd için gerekecek fonksiyonların bulunduğu dosya lcd416.c
#endif
//#device PIC18F4620
int degisim=1, degisim_motor=0, lcd_ekran=1, i=1;
// motor adım ve zaman değişkenler
signed long long mA_step=0, mB_step=0, mC_step=0, mA_step_old=0, mB_step_old=0, mC_step_old=0;
signed long long ma=0, mb=0, mc=0;
long long ta=0,tb=0,tc=0;
// inverse kinematic değişkenleri
float e,f,re,rf,x0,y0,z0,x02,x03,y1,y01,y02,y03,z02,z03,theta1,theta2,theta3,a,b,d,yj,zj;
float hassasiyet=1,tahvil_orani=0;
//********** write eeprom - float ********************************************//
void ee_write_float(unsigned int addr , float *floatptr)
{
unsigned char edata;
unsigned char I;
for(I=0;I<4;I++){
edata = *((unsigned char *)floatptr+I);
write_eeprom(addr+I,edata);
}
}
//********** read eeprom - float *********************************************//
void ee_read_float(unsigned int addr , float *floatptr)
{
unsigned char edata;
unsigned char I;
for(I=0;I<4;I++){
edata=read_eeprom(I+addr);
*((unsigned char *)floatptr+I) = edata;
}
}
//********** write eeprom - long *********************************************//
void ee_write_long(unsigned int addr , signed long long *longptr)
{
unsigned char edata;
unsigned char I;
for(I=0;I<4;I++){
edata = *((unsigned char *)longptr+I);
write_eeprom(addr+I,edata);
}
}
//********** read eeprom - long **********************************************//
void ee_read_long(unsigned int addr , signed long long *longptr)
{
unsigned char edata;
unsigned char I;
for(I=0;I<4;I++){
edata=read_eeprom(I+addr);
*((unsigned char *)longptr+I) = edata;
}
}
//********** çalışma uzayı dışında bir nokta girildiğinde ********************//
void no_point()
{
lcd_send_byte(0,0x01); // lcd ekranı temzile
lcd_gotoxy(1,1);
printf(lcd_putc, "hata: calisma");
lcd_gotoxy(1,2);
printf(lcd_putc, "uzayi icinde bir");
lcd_gotoxy(1,3);
printf(lcd_putc, "nokta giriniz");
delay_ms(2000);
}
//******* kolların açı değerlerini hesaplama: theta1, theta2, theta3 *********//
void hesapla_aci()
{
//********** theta 1 ******************************************************//
y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
y01 = y0 -0.5 * 0.57735 * e; // shift center to edge
// z = a + b*y
a = (x0*x0 + y01*y01 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
b = (y1-y01)/z0;
// discriminant
d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
if (d > 0)
{
yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
zj = a + b*yj;
theta1 = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); // yj, y1 den küçükse 180; degilse 0 değerini çıkarır
}
else{no_point();}
//********** theta 2 ******************************************************//
x02 = x0*cos(120*pi/180) + y0*sin(120*pi/180);
y02 = y0*cos(120*pi/180) - x0*sin(120*pi/180);
z02 = z0;
//y1 = cos(120)*(-0.5 * 0.57735 * f);
y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
y02 = y02 -0.5 * 0.57735 * e; // shift center to edge
// z = a + b*y
a = (x02*x02 + y02*y02 + z02*z02 +rf*rf - re*re - y1*y1)/(2*z02);
b = (y1-y02)/z02;
// discriminant
d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
if (d > 0)
{
yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
zj = a + b*yj;
theta2 = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); //yj, y1 den küçükse 180; degilse 0 değerini çıkarır
}
else{no_point();}
//********** theta 3 ******************************************************//
x03 = x0*cos(120*pi/180) - y0*sin(120*pi/180);
y03 = y0*cos(120*pi/180) + x0*sin(120*pi/180);
z03 = z0;
y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
y03 = y03 -0.5 * 0.57735 * e; // shift center to edge
// z = a + b*y
a = (x03*x03 + y03*y03 + z03*z03 +rf*rf - re*re - y1*y1)/(2*z03);
b = (y1-y03)/z03;
// discriminant
d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
if (d > 0)
{
yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
zj = a + b*yj;
theta3 = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); //yj, y1 den küçükse 180; degilse 0 değerini çıkarır
}
else{no_point();}
}
//********** Motor adım ve zaman bilgilerinin hesaplanıp eproma kaydı ********//
void motor_hareket()
{
mA_step = theta1 / tahvil_orani; // adım değerleri tam sayı cıkmıyor.
mB_step = theta2 / tahvil_orani; // cıkması için int olarak tanımlanması gerekiyor.
mC_step = theta3 / tahvil_orani; // eğer tanımlanırsa 10.85 cıkan adım değeri 10 olarak alınıyor
//********** motora gönderilecek adım değerlerinin hesaplanması ***********//
// motor A
if(mA_step >= 0)
{
if(mA_step > mA_step_old) // motor ileri
{
ma = mA_step - mA_step_old; // ma positive
mA_step_old = mA_step;
}
else // motor geri
{
ma = mA_step - mA_step_old; // ma negative
mA_step_old = mA_step;
}
}
else // mA_step < 0
{
if(mA_step < mA_step_old) // motor geri
{
ma = mA_step - mA_step_old;
mA_step_old = mA_step; // ma negative
}
else // motor ileri
{
ma = mA_step - mA_step_old; // ma positive
mA_step_old = mA_step;
}
}
// motor B
if(mB_step >= 0)
{
if(mB_step > mB_step_old) // motor ileri
{
mb = mB_step - mB_step_old; // mb positive
mB_step_old = mB_step;
}
else // motor geri
{
mb = mB_step - mB_step_old; // mb negative
mB_step_old = mB_step;
}
}
else // mB_step < 0
{
if(mB_step < mB_step_old) // motor geri
{
mb = mB_step - mB_step_old;
mB_step_old = mB_step; // mb negative
}
else // motor ileri
{
mb = mB_step - mB_step_old; // mb positive
mB_step_old = mB_step;
}
}
// motor C
if(mC_step >= 0)
{
if(mC_step > mC_step_old) // motor ileri
{
mc = mC_step - mC_step_old; // mc positive
mC_step_old = mC_step;
}
else // motor geri
{
mc = mC_step - mC_step_old; // mc negative
mC_step_old = mC_step;
}
}
else // mC_step < 0
{
if(mC_step < mC_step_old) // motor geri
{
mc = mC_step - mC_step_old;
mC_step_old = mC_step; // mc negative
}
else // motor ileri
{
mc = mC_step - mC_step_old; // mc positive
mC_step_old = mC_step;
}
}
// en büyük adımı bulma ve ona göre zaman değişkenlerini hesaplama//
if(labs(mb) > labs(mc)) // labs(): long türünden bir sayısının mutlak değerini al
{
if(labs(mb) > labs(ma))
{
// b en büyük
tb = 7; // tb = 15ms
ta = (labs(mb)/labs(ma))*tb;
tc = (labs(mb)/labs(mc))*tb;
}
else
{
// a en büyük
ta = 7; // ta = 15ms
tb = (labs(ma)/labs(mb))*ta;
tc = (labs(ma)/labs(mc))*ta;
}
}
else
{
if(labs(mc) > labs(ma))
{
// c en büyük
tc = 7; // tc = 15ms
ta = (labs(mc)/labs(ma))*tc;
tb = (labs(mc)/labs(mb))*tc;
}
else
{
// a en büyük
ta = 7; // ta = 15ms
tb = (labs(ma)/labs(mb))*ta;
tc = (labs(ma)/labs(mc))*ta;
}
}
//****** eeroma adım bilgilerini ve açı değerlerini kaydet *******//
ee_write_float(0x00, &x0); //delay_ms(10); // konum degerlerini eproma yaz
ee_write_float(0x04, &y0); //delay_ms(10);
ee_write_float(0x08, &z0); //delay_ms(10);
ee_write_long(0x10, &mA_step_old); //delay_ms(10);
ee_write_long(0x14, &mB_step_old); //delay_ms(10);
ee_write_long(0x18, &mC_step_old); //delay_ms(10);
//***** adım bilgileri ve zaman değişkenlerini sırayla gönder ****//
// A motoruna gönder
output_low(PIN_C0); // S0=0
output_low(PIN_C1); // S1=0
delay_us(100);
printf("%ld\r",ma);
printf("%ld\r",ta);
delay_ms(1);
// B motoruna gönder
output_low(PIN_C0); // S0=0
output_high(PIN_C1); // S1=1
delay_us(100);
printf("%ld\r",mb);
printf("%ld\r",tb);
delay_ms(1);
// C motoruna gönder
output_high(PIN_C0); // S0=1
output_low(PIN_C1); // S1=0
delay_us(100);
printf("%ld\r",mc);
printf("%ld\r",tc);
delay_ms(1);
// PC ye gönder
output_high(PIN_C0); // S0=1
output_high(PIN_C1); // S1=0
delay_us(100);
printf("data gonderildi: mA:%ld, ta:%ld, mB:%ld, tb:%ld, mC:%ld, tc:%ld\n\r", ma, ta, mb, tb, mc, tc);
delay_ms(1);
}
//************************* ANA PROGRAM **************************************//
void main()
{
setup_PSP(PSP_DISABLED);
setup_spi(SPI_SS_DISABLED);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_adc_ports(NO_ANALOGS);
setup_adc(ADC_OFF);
setup_CCP1(CCP_OFF);
setup_CCP2(CCP_OFF);
set_tris_a(0b00000111); // port a ilk 3 bit giriş
set_tris_c(0b10000000); // port c çıkış RC7-RX giriş
set_tris_d(0b11111111); // port d giriş
set_tris_e(0b00001000); // port d çıkış mclr giriş
output_c(0);
output_d(0);
output_e(0);
lcd_init(); // lcdyi hazırla
//**************** proje bilgilerini ekranda yazdır***************************//
lcd_gotoxy(1,1); // 1.sutun 1.satır
printf(lcd_putc, "MARMARA UNI");
delay_ms(500);
lcd_gotoxy(1,2); // 1.sutun 2.satır
printf(lcd_putc, "MAKINE MUH");
delay_ms(500);
lcd_gotoxy(1,3); // 1.sutun 3.satır
printf(lcd_putc, "DELTA ROBOT PROJ");
delay_ms(500);
lcd_gotoxy(1,4); // 1.sutun 4.satır
printf(lcd_putc, "04.05.2011");
delay_ms(1500);
lcd_send_byte(0,0x01); // lcd ekranı temzile
//************************** robot geometri **********************************//
e = 109.12; // end effector
f = 346.41; // base
re = 323.59; // rod
rf = 133.09; // arm
tahvil_orani = 0.008948849;
//***************** robotu kalibre et ****************************************//
if(input(PIN_A2)==0) // kalibre butonu RA2 basılımı
{
theta1=theta2=theta3=0;
mA_step_old=mB_step_old=mC_step_old=0;
ma=mb=mc=0;
ta=tb=tc=0;
x0=0;
y0=0;
z0=-253.1246;
ee_write_float(0x00, &x0); delay_ms(10); // rototun sıfır pozisyonunda
ee_write_float(0x04, &y0); delay_ms(10);
ee_write_float(0x08, &z0); delay_ms(10);
ee_write_long(0x10, &mA_step_old); delay_ms(10);
ee_write_long(0x14, &mB_step_old); delay_ms(10);
ee_write_long(0x18, &mC_step_old); delay_ms(10);
lcd_gotoxy(1,1);
printf(lcd_putc, "Kalibre edildi.");
delay_ms(200);
lcd_gotoxy(1,2);
printf(lcd_putc, "Robot suanda");
lcd_gotoxy(1,3);
printf(lcd_putc, "sifir pozisyo-");
lcd_gotoxy(1,4);
printf(lcd_putc, "nunda olmali.");
delay_ms(3000);
lcd_send_byte(0, 0x01);
}
//***************** robotun konumunu eepromdan oku ***************************//
ee_read_float(0x00, &x0); // koordinat değerlerini oku
delay_ms(5);
ee_read_float(0x04, &y0);
delay_ms(5);
ee_read_float(0x08, &z0);
delay_ms(5);
ee_read_long(0x10, &mA_step_old); // eski değerleri oku
delay_ms(5);
ee_read_long(0x14, &mB_step_old);
delay_ms(5);
ee_read_long(0x18, &mC_step_old);
delay_ms(5);
hesapla_aci();
degisim=1;
//******************************* Ana Döngü **********************************//
while(TRUE)
{
//************************* buton taraması ********************************//
if(input(PIN_A3)==1) // motorlar duruyorsa buton taramasını geç
{
if(input(PIN_D0)==0) // +x doğrultusunda 1mm git
{
delay_ms(20);
x0+=hassasiyet;
degisim=1;
degisim_motor=1;
}
if(input(PIN_D1)==0) // -x doğrultusunda 1mm git
{
delay_ms(20);
x0-=hassasiyet;
degisim=1;
degisim_motor=1;
}
if(input(PIN_D2)==0) // +y doğrultusunda 1mm git
{
delay_ms(20);
y0+=hassasiyet;
degisim=1;
degisim_motor=1;
}
if(input(PIN_D3)==0) // -y doğrultusunda 1mm git
{
delay_ms(20);
y0-=hassasiyet;
degisim=1;
degisim_motor=1;
}
if(input(PIN_D4)==0) // +z doğrultusunda 1mm git
{
delay_ms(20);
z0+=hassasiyet;
degisim=1;
degisim_motor=1;
}
if(input(PIN_D5)==0) // -z doğrultusunda 1mm git
{
delay_ms(20);
z0-=hassasiyet;
degisim=1;
degisim_motor=1;
}
}
if(input(PIN_D6)==0) // ekranı değiştir
{
delay_ms(500); // butondaki arkı söndür
lcd_ekran = lcd_ekran + 1;
degisim=1;
if(lcd_ekran == 5){lcd_ekran=1;}
}
if(input(PIN_D7)==0) // hassasiyet arttır butonu
{
delay_ms(500); // butondaki arkı söndür aksi halde artıs muazzam boyutta olur
i+=1; // 4 kademeli hassasiyet artımı
if(i >= 6){i=1;}
switch(i)
{
case 1: hassasiyet = 0.01; break; // 10 micron
case 2: hassasiyet = 0.1; break;
case 3: hassasiyet = 1; break;
case 4: hassasiyet = 10; break;
case 5: hassasiyet = 100; break; // 10 cm
}
degisim=1;
lcd_ekran=4;
}
//***** değişim oldugunda ekranı yenile ve bilgileri eproma kaydet *****//
if(degisim == 1)
{
degisim = 0;
//******** step motor kontrol ***************************************//
if(degisim_motor == 1) // sadece motor verileri değiştiginde
{
degisim_motor = 0;
hesapla_aci(); // alınan değerlere göre açı değerlerini hesapla
motor_hareket(); // motoru hareket ettir
}
//******************** Ekran Görünümü *******************************//
switch(lcd_ekran) // istenilen ekranları göster
{
case 1: // posiyon bilgilerini göster
degisim = 0;
lcd_send_byte(0,0x01); // ekranı temizle
lcd_gotoxy(1,1); // SUTUN , SATIR
printf(lcd_putc, "POSITION (mm)");
lcd_gotoxy(1,2);
printf(lcd_putc, "X: %4.4f", x0); // %4.4f= float türünden bir sayıyı xxxx.xxxx şeklinde ekrana yansıtır
lcd_gotoxy(1,3);
printf(lcd_putc, "Y: %4.4f", y0);
lcd_gotoxy(1,4);
printf(lcd_putc, "Z: %4.4f", z0); break;
case 2: // acı bilgilerini göster
degisim=0;
lcd_send_byte(0,0x01); // ekranı temizle
lcd_gotoxy(1,1); // SUTUN , SATIR
printf(lcd_putc, "ANGLE (derece)");
lcd_gotoxy(1,2);
printf(lcd_putc, "A: %4.6f", theta1); // %f : float türünden
lcd_gotoxy(1,3);
printf(lcd_putc, "B: %4.6f", theta2); // %E : virgülden sonra 6 hane ve E00 seklinde gösterimi
lcd_gotoxy(1,4);
printf(lcd_putc, "B: %4.6f", theta3); break;
case 3: // step motor adım bilgilerini göster
degisim=0;
lcd_send_byte(0,0x01);
lcd_gotoxy(1,1);
printf(lcd_putc, "STEP COUNT(step)");
lcd_gotoxy(1,2);
printf(lcd_putc, "MA: %ld", mA_step); // %ld long ifade decimal türünden
lcd_gotoxy(1,3);
printf(lcd_putc, "MB: %ld", mB_step);
lcd_gotoxy(1,4);
printf(lcd_putc, "MC: %ld", mC_step); break;
case 4: // hassasiyet bilgisini göster
degisim=0;
lcd_send_byte(0,0x01);
lcd_gotoxy(1,1);
printf(lcd_putc, "HASSASIYET");
lcd_gotoxy(1,2);
printf(lcd_putc, "0.01 < H < 100");
lcd_gotoxy(1,3);
printf(lcd_putc, "arasinda olmali");
lcd_gotoxy(1,4);
printf(lcd_putc, "H: %f mm", hassasiyet); break;
}
}
}
}
Yavru pic’ lerin programı
///////////////////////////////////////////////////////////////////////////////
//// xyz_kontrol_16F628A.C ////
//// ////
//// Bu porgram xyz_kontrol_18F4620.C programından gönderilen step motor ////
//// hareket için adım ve zaman gecikme bilgilerini alarak motora hareket ////
//// vermeyi mümkün kılar. ////
//// ////
//// Robot geometry: ////
//// e = 109.12; // end effector ////
//// f = 346.41; // base ////
//// re = 323.59; // rod ////
//// rf = 133.09; // arm ////
//// ////
//// Kullanılan kontrolor 16F628A(2K, 4MHz) ////
//// ////
//// Step Motor bağlantıları: PORTA ////
//// A: RA0 Step motor kontrol bitleri ////
//// B: RA1 ////
//// C: RA2 ////
//// D: RA3 ////
//// ////
///////////////////////////////////////////////////////////////////////////////
//// YAZAN: Remzi ŞAHİNOĞLU ////
//// TARİH: 05.05.2011 ////
//// ////
///////////////////////////////////////////////////////////////////////////////
#if defined(__PCM__)
#include <16F628A.h>
#include <MATH.h>
#use delay(clock=4000000)
#fuses XT, MCLR, PUT, NOWDT, NOLVP, NOBROWNOUT, NOPROTECT
#use rs232 (baud=9600, xmit=PIN_B2, rcv=PIN_B1, parity=N, stop=1)
#include <stdlib.h>
#include <input.c>
#endif
//#device PIC16F628A
int i=0, degisim=0, a=2;
signed long long m=0;
long long t=0;
#int_rda
void serihaberlesme_kesmesi() // \r=0D ENTER-SATIR BAŞI, \n=0A SATIR ATLA
{
disable_interrupts(int_rda);
output_high(PIN_B4);
i = i+1;
if(i==3){i=1;}
if(i==1)
{
m=get_long();
}
if(i==2)
{
degisim = 1;
t=get_long();
output_low(PIN_B4);
}
output_low(PIN_B4);
}
void main()
{
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_CCP1(CCP_OFF);
output_a(0);
output_low(PIN_B0);
output_low(PIN_B3);
output_low(PIN_B4);
output_low(PIN_B5);
output_low(PIN_B6);
output_low(PIN_B7);
degisim=0;
m=0;
t=0;
a = read_eeprom(0x00);
enable_interrupts(GLOBAL);
while(TRUE)
{
enable_interrupts(int_rda);
//***** motor döngüsü **************************************************//
if(degisim == 1)
{
degisim=0;
disable_interrupts(int_rda);
output_high(PIN_B7); // işlem bitti ledi aktif
if(m >= 0)
{
while(m > 0) // positive oldugu sürece motor geri
{
m-=1;
a+=1; // motor ileri dön
if(a>=5){a=1;}
switch (a)
{
case 1: output_a(1); break;
case 2: output_a(2); break;
case 3: output_a(4); break;
case 4: output_a(8); break;
}
delay_ms(t);
}
}
if(m <= 0)
{
while(m < 0) // negative oldugu sürece motor geri
{
m+=1;
a-=1; // motor geri dön
if(a<=0){a=4;}
switch (a)
{
case 1: output_a(1); break;
case 2: output_a(2); break;
case 3: output_a(4); break;
case 4: output_a(8); break;
}
delay_ms(t);
}
}
output_a(0);
output_low(PIN_B7); // işlem bitti ledi pasif
write_eeprom(0x00, a);
}
}
}
Sadece kumandadan gelen komutlar sayesinde hareket ettirilebilir. Bilgisyar kontrolü pic kodunun içine dahil edilmemiştir. Bu yüzden Paralel Robot Elektronik Step Motor sisteminin bilgisayar kontrolü yoktur.
Bir sonraki yazımızda Paralel Robotu Servo Motor ile Tahrik etmeyi deniyeceğiz.
0 yorum