Hướng dẫn viết code điều khiển cánh tay robot gắp phôi từ băng tải

Chào các bạn 220von chia sẻ với các bạn code điều khiển cánh tay gắp phôi từ băng tải

Nhiệm vụ của code như sau :
+ Cho phép chế độ điều khiển bằng tay cánh tay thông qua 4 biến trở
+ Cho phép dạy cho cánh tay cách thức gắp phôi từ băng tải và duy chuyển
+ Cho phép chế độ tự động khi có phôi thì tự động gắp phôi

Video của mô hình như sau :

Phần cánh tay trong mô hình sử dụng 4 động cơ servo MG995
Vi điều khiển được chọn là PIC16F886

220von sử dụng ngắt timer để tạo xung pwm điều khiển các động cơ servo , ưu điểm của phương pháp này là sử dụng được với các tất cả các vi điều khiển có ngắt timer , nhược điểm là dãi xung ko được mịn nên servo khi chuyển động sẽ ko mịn , rất tốn thời gian xử lí của vi điều khiển

Code được viết trên CCS và 220von sẽ không giải thích các code này, mục đích cho các bạn nào am hiểu về code thì có thể tham khảo và nghiên cứu rồi tự phát triển thêm

Code như sau :

#include <16F887.h>
#device adc=8

#FUSES NOWDT                    //No Watch Dog Timer
#FUSES INTRC_IO                 //Internal RC Osc, no CLKOUT
#FUSES NOPUT                    //No Power Up Timer
#FUSES MCLR                     //Master Clear pin enabled
#FUSES NOPROTECT                //Code not protected from reading
#FUSES NOCPD                    //No EE protection
#FUSES NOBROWNOUT               //No brownout reset
#FUSES IESO                     //Internal External Switch Over mode enabled
#FUSES FCMEN                    //Fail-safe clock monitor enabled
#FUSES NOLVP                    //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NODEBUG                  //No Debug mode for ICD
#FUSES NOWRT                    //Program memory not write protected
#FUSES BORV21                   //Brownout reset at 2.1V
#FUSES RESERVED                 //Used to set the reserved FUSE bits

#use delay(clock=8000000)
char sv1=15,sv2=15,sv3=15,sv4=15;
char servo1[8];
char servo2[8];
char servo3[8];
char servo4[8];

char count=0;
#int_TIMER0
void  TIMER0_isr(void)  // ngắt timer điều khiển 4 servo sau mỗi 100us
{
set_timer0(168);
count++;
if(count>=200)
  {
  output_high(PIN_B7);
  output_high(PIN_B6);
  output_high(PIN_B5);
  output_high(PIN_B4);
  count=0;
  }
 if(count>=sv1)output_low(PIN_B7);
 if(count>=sv2)output_low(PIN_B6);
 if(count>=sv3)output_low(PIN_B5);
 if(count>=sv4)output_low(PIN_B4);

 /////////////////////////////////
}
void write_servo(char val1,char val2, char val3, char val4 ,char pos) // lưu vị trí cánh tay
{
write_eeprom(pos*4 + 1,val1);
write_eeprom(pos*4 + 2,val2);
write_eeprom(pos*4 + 3,val3);
write_eeprom(pos*4 + 4,val4);
delay_ms(20);
}
void read_servo() // đọc vị trí cánh tay
{
char k=0;
for(k=1;k<=8;k++)
    {
     servo1[k-1]=read_eeprom(k*4 + 1);
     servo2[k-1]=read_eeprom(k*4 + 2);
     servo3[k-1]=read_eeprom(k*4 + 3);
     servo4[k-1]=read_eeprom(k*4 + 4);
    }
}
void main()
{
 set_tris_a(0b00001111);
 set_tris_b(0b00001111);
 port_b_pullups (0b00001111);
 output_b(0x00);
 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_2);
 set_timer0(164);
 setup_adc_ports(sAN0|sAN1|sAN2|sAN3|VSS_VDD);
 setup_adc(ADC_CLOCK_INTERNAL);
 enable_interrupts(INT_TIMER0);
 read_servo();
 sv1=servo1[7];sv2=servo2[7];sv3=servo3[7];sv4=servo4[7];
 enable_interrupts(GLOBAL);
 delay_ms(10);
 char vitrihoc=0;
 int1 D0_set=0;
 char i=0;
 char k=0;
 signed int8 value1=0,value2=0,value3=0,value4=0;
 while(true)
 {
 if((input(PIN_B3))&&(input(PIN_B2))&&(input(PIN_B1))&&(input(PIN_B0))) // chế độ điều khiển bằng tay và dạy cho cánh tay cách di chuyển và cách gắp phôi
 {
  set_adc_channel(0);delay_us(10);
  sv1 = 6 + read_adc()/15;
  delay_us(10);
    set_adc_channel(1);delay_us(10);
  sv2 = 6 + read_adc()/15;
  delay_us(10);
    set_adc_channel(2);delay_us(10);
  sv3 = 6 + read_adc()/15;
  delay_us(10);
    set_adc_channel(3);delay_us(10);
  sv4 = 6 + read_adc()/15;
  delay_us(10);

  if((input(PIN_D0)==0)&&(D0_set==1)) // nút nhấn lưu vị trí cho cánh tay
   {
    vitrihoc++;
    D0_set=0;
    if(vitrihoc>8){read_servo();}
    else {write_servo(sv1,sv2,sv3,sv4,vitrihoc);}
 
  }
  if(input(PIN_D0))D0_set=1;

 }
 else {/// chế độ chạy và gắp phôi tự động

 if(input(PIN_C3)==0) // cảm biến hồng ngoại phát hiện có phôi đang chờ trên băng tải
   {
      vitrihoc=0;
      for(i=0;i<8;i++)
         {
         if(i==0){sv1=servo1[0];sv2=servo2[0];sv3=servo3[0];sv4=servo4[0];delay_ms(200);}
         else
           {
           value1= servo1[i] - servo1[i-1];
           value2= servo2[i] - servo2[i-1];
           value3= servo3[i] - servo3[i-1];
           value4= servo4[i] - servo4[i-1];
           for(k=0;k<15;k++)
             {
              if(sv1==servo1[i]){value1=0;}
              else
               {
                if(value1>0)sv1++;
                else if(value1<0)sv1--;
                else sv1 = servo1[i];
               }
//////////////////////////////////////////////////////////////////////////////////
              if(sv2==servo2[i]){value2=0;}
              else
               {
                if(value2>0)sv2++;
                else if(value2<0)sv2--;
                else sv2 = servo2[i];
               }
//////////////////////////////////////////////////////////////////////////////////
              if(sv3==servo3[i]){value3=0;}
              else
               {
                if(value3>0)sv3++;
                else if(value3<0)sv3--;
                else sv3 = servo3[i];
               }
//////////////////////////////////////////////////////////////////////////////////
              if(sv4==servo4[i]){value4=0;}
              else
               {
                if(value4>0)sv4++;
                else if(value4<0)sv4--;
                else sv4 = servo4[i];
               }
//////////////////////////////////////////////////////////////////////////////////
             delay_ms(50);
             }
           }
         }
   }
         
     }
   

 }
}

Cám ơn các bạn đã xem , hy vọng code 220von viết hữu ích với các bạn