Line Follower Robot Berbasis Sensor Kamera

Robot dirancang mempunyai sensor kamera, yaitu webcam. Data citra diakuisisi menggunakan komputer lewat software MATLAB. Di komputerlah semua algoritma utama dibuat. Komputer menghasilkan output data PWM yang kemudian dikirim ke mikrokontroler lewat koneksi USB2Serial. Mikrokontroler akan menerjemahkan data yang diterima dari komputer menjadi sinyal PWM. Sinyal ini masuk ke motor driver baru kemudian masuk ke motor dengan tegangan maksimal tegangan baterai, yaitu 11,1V. Robot juga dirancang untuk dapat mengenali posisi dengan mengenali landmark yang terdapat pada beberapa posisi pada garis. Dengan menambahkan data peta garis ke dalam sistem pengolahan data, robot dapat bergerak otomatis ke posisi yang diinginkan atau disebut posisi target.

Robot mendeteksi garis dengan menggunakan metode filter warna. Metode filter warna juga digunakan untuk mendeteksi keberadaan landmark. Sebelum landmark dapat diproses untuk dikenali, citra landmark yang sudah dicrop dirotate dengan berdasarkan data kemiringan yang terdeteksi oleh Transformasi Hough. Dengan demikian input untuk proses pengenalan landmark menjadi sesuai dengan kriteria. Algoritma pengenalannya menggunakan metode PCA (Principle Component Analysis) yang terlebih dahulu membuat data latih.

Berikut skematik sistem mikrokontrolernya.

sch_con
_________________________________________________
DAFTAR PUSTAKA
[1] Armbrust , C. Braun , T. Föhst, T. Proetzsch, M. Renner, A. Schäfer, B.H., Berns K. The Robust Autonomous Vehicle for Off-road Navigation. Technische Universität Kaiserslautern.
[2] Bräunl, Thomas. EMBEDDED ROBOTICS: Mobile Robot Design and Applicationswith Embedded Systems. Springer-Verlag Berlin Heidelberg 2003, 2006. Germany.
[3] Datasheet mikrokontroler ATmega32.
[4] Datasheet IC H-Bridge L298.
[5] Francois Dupuis, Jean. Parizeau, Marc. Evolving a Vision-Based LineFollowing Robot Controller. Departement de genie electrique et de genie informatique. Universite Laval, Quebec. Canada.
[6] Kim, Kyungnam.Face Recognition using Principle Component Analysis. Department of Computer Science. University of Maryland, College Park MD 20742, USA.
[7] Pitowarno, Endra.2006.”ROBOTIKA: Desain, Kontrol, dan Kecerdasan Buatan”. Percetakan Andi Offset. Yogyakarta. Indonesia.
[8] Pranata Kusuma, Andri. 2013. Analysis Of Hough Transform Algorithm For Circle And Ellipse Detection Based On Digital Image Processing. Tugas Akhir Program Sarjana. Institut Teknologi Telkom.
[9] Rusdinar, Angga. Kim, Jungmin. Lee, Junha. Kim, Sungshin. 2011. Implementation Of Real-Time Positioning System Using Extended Kalman Filter And Artificial Landmark On Ceiling. School of Electrical Engineering, Pusan National University, Busan, 609-735, Korea.
[10] Schworer, Ian. 2005.Navigation and Control of an Autonomous Vehicle. Virginia Polytechnic Institute and State University. Blacksburg,Virginia.
[11] Sigit, Riyanto. 2007. Robotikaa, Sensor, dan Aktuator. Yogyakarta : Graha Ilmu.
[12] W. Wall, Richard. Bennett, Jerry. Eis, Greg. Lichy, Kevin. Owings, Elizabeth. Creating A Low-Cost Autonomous Vehicle. University of Idaho Moscow.

____________________________________________________
LAMPIRAN
Program berikut adalah program yang ditanamkan pada mikrokontroler ATmega8535 dengan compiler CodeVisionAVR.
A. Program Utama Mikrokontroler ATmega8535

/*****************************************************
Chip type           : ATmega32
Program type        : Application
Clock frequency     : 4.000000 MHz
Memory model        : Small
External SRAM size  : 0
Data Stack size     : 512
*****************************************************/

#include <mega32.h>
#include "LED_lib.c"
#include "motor_lib.c"
#include "common_lib.c" 
#include "serial_com_lib.c"

#include <string.h>
char command[32];
int value;
void main(void)
{
LED_init();
motor_init();  
serial_init();
common_init();

while (1)
      { 
        scanf("robot=%d:%s",&value,command);     
        if      (!(strcmpf(command,"LED_1")))
        {
                LED_1 = value;
                //printf("LED_1 = %d",value);        
        }
        else if (!(strcmpf(command,"LED_2")))
        {
                LED_2 = value;
                //printf("LED_2 = %d",value);        
        }  
        else if (!(strcmpf(command,"LED_3")))
        {
                LED_3 = value;
                //printf("LED_3 = %d",value);        
        }
        else if (!(strcmpf(command,"LED_4")))
        {
                LED_4 = value;
                //printf("LED_4 = %d",value);        
        }
        else if (!(strcmpf(command,"LED_5")))
        {
                LED_5 = value;
                //printf("LED_5 = %d",value);        
        }
        else if (!(strcmpf(command,"LED_6")))
        {
                LED_6 = value;
                //printf("LED_6 = %d",value);        
        }
        else if (!(strcmpf(command,"LED_7")))
        {
                LED_7 = value;
                //printf("LED_7 = %d",value);        
        }
        else if (!(strcmpf(command,"motor_A")))
        {
                motor_A(value);
                //printf("motor_A = %d",value);        
        }
        else if (!(strcmpf(command,"motor_B")))
        {
                motor_B(value);
                //printf("motor_B = %d",value);        
        } 
      };
}

B.Subprogram common_lib.c

void common_init()
{
        // Analog Comparator initialization
        // Analog Comparator: Off
        // Analog Comparator Input Capture by Timer/Counter 1: Off
        ACSR=0x80;
        SFIOR=0x00;
}

C.Subprogram LED_lib.c

#define LED_1           PORTB.0         
#define LED_2           PORTB.1         
#define LED_3           PORTB.2         
#define LED_4           PORTB.3         
#define LED_5           PORTB.4         
#define LED_6           PORTB.5         
#define LED_7           PORTB.6           

#define DDR_LED         DDRB

void LED_init()
{
        DDR_LED = 0xFF;                
}

D.Subprogram motor_lib.c

#include <math.h>
#define A_PWM           OCR1AL   
#define DIR_A_PWM       DDRD.5
#define DIR_A1          PORTC.4
#define DIR_A2          PORTC.5
#define DDR_DIR_A1      DDRC.4 
#define DDR_DIR_A2      DDRC.5   

#define B_PWM           OCR1BL   
#define DIR_B_PWM       DDRD.4
#define DIR_B1          PORTC.6
#define DIR_B2          PORTC.7
#define DDR_DIR_B1      DDRC.6 
#define DDR_DIR_B2      DDRC.7

void motor_init()
{
        DDR_DIR_A1 = 1;
        DDR_DIR_A2 = 1;
        DIR_A_PWM  = 1;

        DDR_DIR_B1 = 1;
        DDR_DIR_B2 = 1;
        DIR_B_PWM  = 1;

        // Timer/Counter 1 initialization
        // Clock source: System Clock
        // Clock value: 3.906 kHz
        // Mode: Fast PWM top=00FFh
        // OC1A output: Non-Inv.
        // OC1B output: Non-Inv.
        // Noise Canceler: Off
        // Input Capture on Falling Edge
        // Timer 1 Overflow Interrupt: Off
        // Input Capture Interrupt: Off
        // Compare A Match Interrupt: Off
        // Compare B Match Interrupt: Off
        TCCR1A=0xA1;
        TCCR1B=0x0D;
        TCNT1H=0x00;
        TCNT1L=0x00;
        ICR1H=0x00;
        ICR1L=0x00;
        OCR1AH=0x00;
        OCR1AL=0x00;
        OCR1BH=0x00;
        OCR1BL=0x00;
}       

void motor_A(int pwm_input)
{
        if(pwm_input &gt;= 0)     
        {                                        
                DIR_A1 = 1;
                DIR_A2 = 0;
                A_PWM  = (unsigned char)pwm_input;
        }
        else
        {                                        
                DIR_A1 = 0;
                DIR_A2 = 1;
                A_PWM = (unsigned char)(abs(pwm_input));  
        }         
}     

void motor_B(int pwm_input)
{
        if(pwm_input &gt;= 0)     
        {                                        
                DIR_B1 = 1;
                DIR_B2 = 0;
                B_PWM  = (unsigned char)pwm_input;
        }
        else
        {                                        
                DIR_B1 = 0;
                DIR_B2 = 1;
                B_PWM = (unsigned char)(abs(pwm_input));  
        }         
}

E.Subprogram serial_com_lib.c

#include <stdio.h>

#define RXB8 1
#define TXB8 0
#define UPE 2
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7

#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<OVR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)

// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];

#if RX_BUFFER_SIZE<256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif

// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;

// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
   {
   rx_buffer[rx_wr_index]=data;
   if (++rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
   if (++rx_counter == RX_BUFFER_SIZE)
      {
      rx_counter=0;
      rx_buffer_overflow=1;
      };
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index];
if (++rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif

// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];

#if TX_BUFFER_SIZE<256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif

// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
   {
   --tx_counter;
   UDR=tx_buffer[tx_rd_index];
   if (++tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
   {
   tx_buffer[tx_wr_index]=c;
   if (++tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
   ++tx_counter;
   }
else
   UDR=c;
#asm("sei")
}
#pragma used-
#endif      

void serial_init()
{
        // USART initialization
        // Communication Parameters: 8 Data, 1 Stop, No Parity
        // USART Receiver: On
        // USART Transmitter: On
        // USART Mode: Asynchronous
        // USART Baud rate: 19200
        UCSRA=0x00;
        UCSRB=0xD8;
        UCSRC=0x86;
        UBRRH=0x00;
        UBRRL=0x0C;

        // Global enable interrupts
        #asm("sei")
}                 

Berikut adalah program utama yang dijalankan pada komputer dengan software Matlab.
A.Program Utama main.m

clc
clear all
close all
%----load database huruf
load Eigen_data.mat
%inisialisasi kecepatan rata-rata
v_avr = 45;
%konstanta kecepatan motor
kp = 6;
kd = 5;
%*******************inisialisasi serialport*****************************
delete(instrfindall); 
S1 = serial ('COM2', 'Baudrate', 19200, 'DataBits', 8, 'Parity','none'); 
fopen(S1);
%*******************inisialisasi camera*****************************
imaqreset
vid = videoinput('winvideo',2,'YUY2_160x120');
set(vid,'ReturnedColorSpace','rgb');
set(vid,'TriggerRepeat',Inf);
set(vid,'FramesPerTrigger',1);
triggerconfig(vid, 'Manual');
start(vid)
% figure,
%*******************SUPER LOOP**************************************
motor_A = 0;
motor_B = 0;   
error_prev = 0;
prev_centroid = 4.5;
loging_data = [];
%--------------auto kalibrasi-------------
trigger(vid);
im = getdata(vid,1);
thrs = auto_kalibrasi(im);
figure,imshow(im);
log_data = [];
%-----------map init---------------------
target_pos = 8;
prev_pos = 3;
path_idx = [];
for zz=1:100
    tic
    trigger(vid);
    data = getdata(vid,1);
    %********ikuti garis***********
    [data_sens1 data_sens2] = block_sens(data,thrs);
    centroid = median(data_sens1)
    if(centroid == 0)
        centroid = prev_centroid;
    else
        prev_centroid = centroid;
    end

    error = centroid - 4.5;
    delta_error =  error - error_prev;
    motor_A = round(v_avr + (error*kp) + (delta_error*kd));
    motor_B = round(v_avr - (error*kp) + (delta_error*kd));
    error_prev = error;

    if(motor_A > 255)
        motor_A = 255;
    elseif(motor_A < -255)
        motor_A = -255;
    end
    %batasi nilai PWM motorB
    if(motor_B > 255)
        motor_B = 255;
    elseif(motor_B < -255)
        motor_B = -255;
    end
    %--------cek landmark----------------
    im = data;
    crop_area = im(31:120,1:160,:);
    [bb bc c] = color_track(crop_area);
    c
    if(c>0)
        error = 0;
        error_prev = 0;
        motor_A = -50;
        motor_B = -50;
        data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
        fprintf(S1,data_format); 
        data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
        fprintf(S1,data_format);
        pause(0.2);
        motor_A = 0;
        motor_B = 0;
        data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
        fprintf(S1,data_format); 
        data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
        fprintf(S1,data_format);
        %-------------------crop landmark--------------------
        pause(3);
        trigger(vid);
        im = getdata(vid,1);
        [bb bc c] = color_track(im);
        get_landmark = imcrop(im,bb);
        figure,imshow(get_landmark);
        pause(2);
        ok_landmark = hou(get_landmark);
        %-----------------kenali landmark-----------------
        figure,imshow(ok_landmark);
        data = ok_landmark(:,:,3);
        rata_oy = mean(mean(data));
        if(rata_oy>200)
            data = data*0.8;
        end
        ukur_oy = size(data);
        for iii=1:ukur_oy(1)
            for jjj=1:ukur_oy(2)
                if data(iii,jjj) < 200
                   data(iii,jjj) = 0;
                end
            end
        end
        figure,imshow(data);
        im_crop = data;
        im_test = imresize(im_crop, [baris kolom]);
        im_vector_test = double(reshape(im_test,[],1));
        A_test = im_vector_test - m;
        im_proyeksi_test = Eigen' * A_test;
        for i=1:jum_data
            Euc_distance(:,i) = norm( im_proyeksi_test - im_proyeksi_latih(:,i) );
        end
        [Euc_dist_min , Recognized_index] = min(Euc_distance);
        %-----------------map direction----------------------
        Recognized_index
        pause(3)
        close all
        if(Recognized_index == target_pos)
            disp('FINISH');
            break;
        end
        Recognized_index
        target_pos
        prev_pos
        derajat_belok = find_path_v2(Recognized_index,target_pos,prev_pos)       
        prev_pos = Recognized_index;
        pause(2)
        if(derajat_belok == 0)
            while(c>0)
                trigger(vid);
                data = getdata(vid,1);
                crop_area = data(41:120,1:160,:);
                [bb bc c] = color_track(crop_area);
                c
                motor_A = 43;
                motor_B = 45;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format); 
            end
            motor_A = 0;
            motor_B = 0;
            data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
            fprintf(S1,data_format); 
            data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
            fprintf(S1,data_format);
        elseif(derajat_belok == 90)
            while(c>0)
                trigger(vid);
                data = getdata(vid,1);
                [bb bc c] = color_track(data);
                motor_A = 43;
                motor_B = 45;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format);
            end
            trigger(vid);
            data = getdata(vid,1);
            [data_sens1 data_sens2] = block_sens(data,thrs);
            cc  = numel(data_sens2);
            while(cc>0)
                trigger(vid);
                data = getdata(vid,1);
                [data_sens1 data_sens2] = block_sens(data,thrs);
                cc  = numel(data_sens2);
                motor_A = 43;
                motor_B = 0;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format); 
            end
            trigger(vid);
            data = getdata(vid,1);
            [data_sens1 data_sens2] = block_sens(data,thrs);
            cc  = numel(data_sens2);
            while(cc<4)
                trigger(vid);
                data = getdata(vid,1);
                [data_sens1 data_sens2] = block_sens(data,thrs);
                cc  = numel(data_sens2)
                motor_A = 43;
                motor_B = 0;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format);
            end
            motor_A = 0;
            motor_B = 0;
            data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
            fprintf(S1,data_format); 
            data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
            fprintf(S1,data_format);
            pause(2);
        elseif(derajat_belok == -90)
            while(c>0)
                trigger(vid);
                data = getdata(vid,1);
                [bb bc c] = color_track(data);
                motor_A = 43;
                motor_B = 45;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format);
            end
            trigger(vid);
            data = getdata(vid,1);
            [data_sens1 data_sens2] = block_sens(data,thrs);
            cc  = numel(data_sens2);
            while(cc>0)
                trigger(vid);
                data = getdata(vid,1);
                [data_sens1 data_sens2] = block_sens(data,thrs);
                cc  = numel(data_sens2);
                motor_A = 0;
                motor_B = 43;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format); 
            end
            trigger(vid);
            data = getdata(vid,1);
            [data_sens1 data_sens2] = block_sens(data,thrs);
            cc  = numel(data_sens2);
            while(cc<4)
                trigger(vid);
                data = getdata(vid,1);
                [data_sens1 data_sens2] = block_sens(data,thrs);
                cc  = numel(data_sens2)
                motor_A = 0;
                motor_B = 43;
                data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
                fprintf(S1,data_format); 
                data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
                fprintf(S1,data_format);
            end
            motor_A = 0;
            motor_B = 0;
            data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
            fprintf(S1,data_format); 
            data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
            fprintf(S1,data_format);
            pause(2);
        end
%-----------------keluar rutin-----------------
    end
    %kirim serial
    data_format = ['robot=1:LED_1 \n\r'];
    fprintf(S1,data_format); 

    data_format = ['robot=',num2str(motor_A),':motor_A \n\r'];
    fprintf(S1,data_format); 
    data_format = ['robot=',num2str(motor_B),':motor_B \n\r'];
    fprintf(S1,data_format);

    data_format = ['robot=0:LED_1 \n\r'];
    fprintf(S1,data_format); 
    %********crop mark area******** 
    log_data = [log_data toc];
end
figure,plot(log_data);
data_format = ['robot=0:motor_A \n\r'];
fprintf(S1,data_format); 
data_format = ['robot=0:motor_B \n\r'];
fprintf(S1,data_format);

fclose(S1);
stop(vid)
delete(vid)

B.Fungsi auto_kalibrasi

function [ thrs ] = auto_kalibrasi( im )
im = rgb2gray(im);
kolom_cap = 160;
block_size = 20;
start_block = 101;
mtx_mean = [];
for x=1:block_size:(kolom_cap-block_size+1)
        mtx_mean = [mtx_mean mean(mean(mean(im(start_block:(start_block+block_size-1),x:(x+19)))))];
end
mtx_A = min(mtx_mean);
mtx_B = max(mtx_mean);
val_mtx = [mtx_A mtx_B];
thrs = mean(val_mtx);

C.Fungsi block_sens

function [ cent1 cent2 ] = block_sens( im,thrs )
% im = rgb2gray(im);
im = im(:,:,1);
kolom_cap = 160;
block_size = 20;
mtx_cent1 = [];
mtx_cent2 = [];
start_block = 101;
count = 1;
for x=1:block_size:(kolom_cap-block_size+1)
        rata2 = mean(mean(mean(im(start_block:(start_block+block_size-1),x:(x+19)))));
        if rata2 < thrs
            mtx_cent1 = [mtx_cent1 count];
            mtx_cent2(count) = 1;
        end
        count = count + 1;
end
cent1 = mtx_cent1;
cent2 = mtx_cent2;

D.Fungsi buat_eigenvalue_database

clc
clear all
close all
%buat data base image training
jum_data = 8;
baris = 55;
kolom = 55;
im_gray_buff = [];
for i=1:jum_data
    link_im = ['PCA_latih\',num2str(i),'.jpg'];
    im_rgb = imread(link_im);
    data = im_rgb(:,:,3);
    ukur_oy = size(data);
    for iii=1:ukur_oy(1)
        for jjj=1:ukur_oy(2)
            if data(iii,jjj) < 200
                data(iii,jjj) = 0;
            end
        end
    end
    im_gray_buff = data;
    imshow(im_gray_buff);
    pause(2)
    im_gray = imresize(im_gray_buff,[baris kolom]);
    im_vector_latih(:,i) = double(reshape(im_gray,[],1));
end
m = mean(im_vector_latih,2);
for i=1:jum_data
    A(:,i) = im_vector_latih(:,i) - m;
end
L = A'*A;
[V D] = eig(L);
L_eigen_vector = [];
for i = 1 : size(V,2)
    if( D(i,i)>1 )
        L_eigen_vector = [L_eigen_vector V(:,i)];
    end
end
Eigen = A * L_eigen_vector;
for i=1:jum_data
        im_proyeksi_latih(:,i) = Eigen'*A(:,i);
end
save Eigen_data.mat Eigen A m jum_data baris kolom im_proyeksi_latih
close all

E. Fungsi color_track

function [bb , bc , jum_obj] = color_track(data)
diff_im = imsubtract(data(:,:,1), rgb2gray(data));
diff_im = im2bw(diff_im,0.1);
diff_im = bwareaopen(diff_im,170);
%-------------------------------------------------
ukur_diff_im = size(diff_im);
mtx_idx = [];
stat = 0;
for i=1:ukur_diff_im(1)
    for j=1:ukur_diff_im(2)
        if(diff_im(i,j)==1)
            himp = [j i];
            mtx_idx = [mtx_idx; himp];
            stat = stat + 1;
        end
    end
end
if(stat>0)
    a = min(mtx_idx);
    b = max(mtx_idx);
    for i=a(1):b(1)
        for j=a(2):b(2)
            diff_im(j,i) = 1;
        end
    end
end
%-------------------------------------------------
bw = bwlabel(diff_im, 8);
stats = regionprops(bw, 'BoundingBox', 'Centroid');
jum_obj = length(stats);
bb=0;
bc=0;
for object = 1:length(stats)
     bb = round(stats(object).BoundingBox);
     bc = round(stats(object).Centroid);
end  

F.Fungsi find_path_v2

function [result_angle] = find_path_v2(start_point,target_point,prev_point)
% start_point = 2;
% target_point = 5;
% prev_point = 1;
map = [ 1 2 3 0;
        2 1 4 7;
        3 1 6 7;
        4 2 8 0;
        5 6 8 0;	
        6 3 5 0;	
        7 2 3 8;
        8 4 5 7];

angle_map = [   3	1	2	90;
                2	1	3	-90;
                4	2	1	0;
                7	2	1	-90;
                1	2	4	0;
                7	2	4	90;
                1	2	7	90;
                4	2	7	-90;
                6	3	1	0;
                7	3	1	90;
                1	3	6	0;
                7	3	6	-90;
                1	3	7	-90;
                6	3	7	90;
                8	4	2	-90;
                2	4	8	90;
                8	5	6	90;
                6	5	8	-90;
                5	6	3	-90;
                3	6	5	90;
                3	7	2	0;
                8	7	2	90;
                2	7	3	0;
                8	7	3	-90;
                2	7	8	-90;
                3	7	8	90;
                5	8	4	0;
                7	8	4	-90;
                4	8	5	0;
                7	8	5	90;
                4	8	7	90;
                5	8	7	-90];    

loop_stat = 0;
buffer_point = map(start_point,:);
track_path = [];
prev_point_path = prev_point;
while loop_stat == 0
    buffer_prev = buffer_point(1); 
    stat_find = find(buffer_point(2:end) == target_point);
    dim_find = size(stat_find);
    if(dim_find(2) == 0)
        buff_find = find((buffer_point(2:end) ~= prev_point_path) & (buffer_point(2:end) ~= 0));
        ukur_buff_find = size(buff_find);
        const_rand = ceil(rand(1)*ukur_buff_find(2));
        idx = buff_find(const_rand) + 1;
        buffer_point = buffer_point(idx);
        prev_point_path = buffer_prev;
        track_path = [track_path buffer_point];
        buffer_point = map(buffer_point,:);
    else
            loop_stat = 1;
            track_path = [start_point track_path target_point];
    end
end

track_path = [prev_point track_path];
bar_angle_map = find(ismember(angle_map(:,1:3),track_path(1:3),'rows'),1);
result_angle = angle_map(bar_angle_map,4);

G.Fungsi hou

function [im3] = hou(im)
Igray = rgb2gray(im);
BW2 = edge(Igray,'canny');
[H, theta, rho] = hough(BW2);
peak = houghpeaks(H);
barAngle = theta(peak(2));
im2 = imrotate(im,barAngle,'bilinear','crop');
[bb bc c] = color_track(im2);
im3 = imcrop(im2,bb);

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s