Paralel Robot Elektronik Devre Step Motor Tasarımı – 6

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ı
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
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
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
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
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 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.

Bir cevap yazın

E-posta hesabınız yayımlanmayacak.

This site uses Akismet to reduce spam. Learn how your comment data is processed.