Tải bản đầy đủ - 0 (trang)
Chương 3: Thuật toán dò đường và code

Chương 3: Thuật toán dò đường và code

Tải bản đầy đủ - 0trang

Đồ án hệ thống số

Xác định 1 ma trận mê cung bất kỳ 1 lối vào 1 lối ra

Xây dựng mơ hình robot tự hành có 2 động cơ di chuyển, 1 động

cơ chỉnh góc quay của cảm biến

Sử dụng cảm biến siêu âm

Kết quả: robot có thể di chuyển, nhận tín hiệu từ cảm biến điều chỉnh hướng đi,

đi vào từ lối vào và ra từ lối ra của ma trận

3.2 Thuật toán dò đường

Sử dụng 3 modun thu phát hồng ngoại để xác nhận khoảng cách từ 3 hướng

trái, phải, giữa. Tín hiệu từ 3 biến này được truyền tới kit CPLD xác nhận và đưa tín

hiệu ra 4 chân của 2 động cơ DC.

Ưu tiên rẽ trái rồi đến đi thẳng và rẽ phải nếu cả 3 hướng khơng đi được thì

quay đầu.

Lưu đồ thuật tốn:

Sơ đồ mê cung đơn giản



Hình 3.3. Sơ đồ mê cung đơn giản



Đồ án hệ thống số

3.3. Code

----------------------------------------------------------------------------------- Company: the fisrt

-- Engineer: vando

--- Create Date: 09:05:07 03/22/2013

-- Design Name:



probe line robot



-- Module Name: code2013_03_22 - Behavioral

-- Project Name:



final



-- Target Devices:

-- Tool versions:

-- Description:

--- Dependencies:

--- Revision:

-- Revision 0.01 - File Created

-- Additional Comments:

----------------------------------------------------------------------------------library IEEE;

use IEEE.STD_LOGIC_1164.ALL;



Đồ án hệ thống số

use ieee.std_logic_unsigned.all;



entity code2013_03_22 is

port (

rf1 : in std_logic;

rf2 : in std_logic;

rf4 : in std_logic;

rf3 : in std_logic;

chon: in std_logic;

sensor1 : in std_logic;

sensor2 : in std_logic;

sensor3 : in std_logic;

sensor4 : in std_logic;

clk



: in std_logic;



left1



: out std_logic;



left2 : out std_logic;

right1 : out std_logic;

right2 : out std_logic

);

end code2013_03_22;

architecture Behavioral of code2013_03_22 is

signal clk1 : std_logic;



Đồ án hệ thống số



begin

process (clk)

variable tre1 :integer range 0 to 100000000;

variable tre2 :integer range 0 to 100000000;

variable tre3 :integer range 0 to 100000000;

variable tre4 :integer range 0 to 100000000;

begin

if (clk'event and clk ='1') then--3-4

if( chon ='1' )then

if (sensor3 ='1' and sensor4 ='1') then

tre1 := tre1 +1;

tre2:=0;

tre3:=0;

if (tre1 >= 2400000 and tre1 <=

3200000) then

left1 <= '1';

left2 <= '0';

right1 <= '1';

right2 <= '0';



Đồ án hệ thống số

elsif (tre1 >= 0 and tre1 < 1600000)

then

--quay trai--



left1 <= '1';

left2 <= '0';

right1 <= '0';

right2 <= '1';



else

left1 <= '0';

left2 <= '0';

right1 <= '0';

right2 <= '0';



end if;

tre1 := 0;

--wait until tre >= 5000;

elsif( sensor2 ='1')then --2

--di thang--



Đồ án hệ thống số

tre1:=0;

tre2:=0;

tre3 := tre3 + 1;

if(tre3 >0 and tre3 < 500000) then

left1 <= '1';

left2 <= '0';

right1 <= '1';

right2 <= '0';

elsif(500000 <= tre3 and tre3 <=

700000) then

left1 <= '0';

left2 <= '0';

right1 <= '0';

right2 <= '0';

tre3:=0;

elsif( tre3 > 700000) then

tre3:=3;

else

null;



end if;



Đồ án hệ thống số

elsif (sensor1 ='1') then

tre2 := tre2 +1;

tre1:=0;

tre3:=0;

if (tre2 > 0 and tre2 <= 1600000) then

--re phai-left1 <= '0';

left2 <= '1';

right1 <= '1';

right2 <= '0';



else

left1 <= '0';

left2 <= '0';

right1 <= '0';

right2 <= '0';

end if;

tre2 := 0;

elsif (sensor3 ='0' and sensor4 ='0' and sensor2 ='0'

and sensor1 ='0')then

--tre := tre +1;

--quaydau--



Đồ án hệ thống số

left1 <= '0';

left2 <= '1';

right1 <= '1';

right2 <= '0';

--wait until sensor1 <= '1';

--left1 <= '1';

--left2 <= '0';

--right1 <= '1';

--right2 <= '0';

--tre := 0;

--end if;

else

null;

end if;



elsif(chon='0') then

if( rf1 ='1') then

left1 <= '1';

left2 <= '0';

right1 <= '0';

right2 <= '1';

elsif(rf2='1') then



Đồ án hệ thống số

left1 <= '0';

left2 <= '1';

right1 <= '1';

right2 <= '0';

elsif(rf3='1') then

left1 <= '1';

left2 <= '0';

right1 <= '1';

right2 <= '0';

elsif(rf4 ='1') then



left1 <= '0';

left2 <= '1';

right1 <= '0';

right2 <= '1';

elsif( rf1='0' and rf2='0' and rf3='0' and rf4='0' ) then

left1 <= '0';

left2 <= '0';

right1 <= '0';

right2 <= '0';

else null;

end if;



Đồ án hệ thống số

else null;

end if;

end if;

end process;

end Behavioral;



Mục lục:Tài liệu tham khảo

1.http://letsmakerobots.com/blog

2. linefollower-atmega16-Vienna University.pdf



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

Chương 3: Thuật toán dò đường và code

Tải bản đầy đủ ngay(0 tr)

×