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

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

Mình 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
Code C :

#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);
             }
           }
         }
   }
         
     }
   

 }
}