Đo khoảng cách bằng cảm biến ứng dụng cho Robot

Đăng lúc: Thứ sáu - 26/02/2016 10:17 - Người đăng bài viết: SuperG
Cảm biến siêu âm SRF05

Cảm biến siêu âm SRF05

Để đo khoảng cách, ta có nhiều phương pháp đo khác nhau như siêu âm, hồng ngoại, lazer…
Có 3 cách để đo khoảng cách bằng sóng siêu âm:
Cách đầu tiên là đo độ rộng xung của tín hiệu thu về và áp dụng định luật Doppler để tính ra khoảng cách với tần số phát là 40Khz.
Cách thứ hai là đọc giá trị adc, từ giá trị adc này có thể tính được khoảng cách. Các bạn có thể tham khảo ở trang web này để biết thêm thông tin( http://www.maxbotix.com/articles/032.htm)
Cách thứ ba là đo thời gian từ khi phát xung đến khi nhận xung là bao nhiêu. Áp dụng công thức s=(v*t)/2 với v=340m/s. Ở đây mình dùng cách này.
 
VD Lập trình:
- Dùng timer1 để tính thời gian tràn là 65ms. Mỗi khi tràn thì ta phát đi một chuỗi xung gồm 8 xung với tần số là 40Khz.
Mình có gửi hình ảnh dạng sóng đo bằng oscilloscope.
- Khi có vật cản thì sẽ có tín hiệu đưa về, qua bộ so sánh điện áp thì mình thu được xung vuông. Xung vuông này được đưa đến chân RB0 (ngắt). Mình lập trình mức tích cực là thấp lên cao.
- Để tính được thời gian thì trước khi phát 8 xung ta bật timer1 đồng thời tắt ngắt ngoài vì để tránh tình trạng truyền trực tiếp từ phát sang thu, khi phát xong thì ta mới bật ngắt. Khi có tín hiệu đưa về chân thu thì ta dùng lệnh get_timer1 để tính thời gian.
Ở đây thời gian mỗi lần tràn là 65ms nên giá trị timer1 được tính như sau( pic dùng tần số 12Mhz): timer= 4*(65536-16786)*1/3us=65000us=65ms, vậy giá trị được đưa timer1 là 16786. Do đó khi dùng lệnh get_time để lấy thời gian thì thời gian thật phải được trừ cho 16786. vậy time = timer1-16786
Vì tần số của PIC là 12Mhz nên tần số mỗi chu kỳ lệnh là 12/4. Vì mình mặc định cho timer1 là T1_DIV_BY_4 nên tần số của mỗi lần đếm lên là 12/4/4 bằng 12/16.
Vì vận tốc tính bằng m, thời gian tính bằng us nên đổi ra cm thì ta phải chia cho 10000.
Áp dụng công thức s=(v*t)/2 ta được như sau:
s=(340*time)/(12/16*2*10000)= time/44.
Do trễ thời gian của tín hiệu tính từ chân nhận của cảm biến thu tới chân ngắt RB0 và các lệnh trong chương trình nên ta phải cân chỉnh lại con số 44 thành 59 ( kết quả dựa vào đo thực tế). vậy s=time/59.
 

Hình 1: Cảm biến đo khoảng cách SRF05

 CẢM BIẾN SIÊU ÂM SRF05

Cảm biến SRF05 là một loại cảm biến khoảng cách dựa trên nguyên lý thu phát siêu âm. Cảm biến gồm một bộ phát và một bộ thu sóng siêu âm. Sóng siêu âm từ đầu phát truyền đi trong không khí, gặp vật cản (vật cần đo khoảng cách tới) sẽ phản xạ ngược trở lại và được đầu thu ghi lại. Khoảng cách đo được của SRF05 nằm trong phạm vi từ 4cm đến 300cm.

Hình 2: Nguyên lý thu phát siêu âm


SRF05 có thể thiết lập 2 mode hoạt động khác nhau thông qua các chân điều khiển MODE. Nối hoặc không nối chân MODE xuống GND cho phép cảm biến được điều khiển thông qua giao tiếp dùng 1 chân hay 2 chân IO. Trong demo này,ta sẽ sử dụng mode thứ 2.

Để điều khiển SRF05,đầu tiên xuất một xung với độ rộng tối thiểu 10uS vào chân TRIGGER-ECHO (chân số 3) của cảm biến. Sau đó vi xử lý tích hợp trên cảm biến sẽ phát ra tín hiệu điều khiển đầu phát siêu âm. Sau 700uS kể từ lúc kết thúc tín hiệu điều khiển, từ chân TRIGGER-ECHO có thể đọc ra một xung mà độ rộng tỉ lệ với khoảng cách từ cảm biến tới vật thể.

Hình 6: Nguyên lý hoạt động của SRF05 ở mode 2
 
Cảm biến đo khoảng cách HC-SR04

Cảm biến khoảng cách siêu âm HC-SR04 được sử dụng rất phổ biến để xác định khoảng cách vì RẺ và CHÍNH XÁC. Cảm biến sử dụng sóng siêu âm và có thể đo khoảng cách trong khoảng từ 2 -> 300 cm, với độ chính xác gần như chỉ phụ thuộc vào cách lập trình.

Cảm biến HC-SR04 có 4 chân là: Vcc, Trig, Echo, GND.

Vcc 5V
Trig Một chân Digital output
Echo Một chân Digital input
GND GND
Sơ đồ nối chân giữa HC-SR04 và Arduino
Nguyên lý hoạt động:
 
Để đo khoảng cách, ta sẽ phát 1 xung rất ngắn (5 microSeconds - ú) từ chân Trig. Sau đó, cảm biến sẽ tạo ra 1 xung HIGH ở chân Echo cho đến khi nhận lại được sóng phản xạ ở pin này. Chiều rộng của xung sẽ bằng với thời gian sóng siêu âm được phát từ cảm biển và quay trở lại. 
 
Tốc độ của âm thanh trong không khí là 340 m/s (hằng số vật lý), tương đương với 29,412 microSeconds/cm (106 / (340*100)). Khi đã tính được thời gian, ta sẽ chia cho 29,412 để nhận được khoảng cách.​
 
Lắp mạch:
 
Lập trình trên Arduino:
 
const int trig = 7;     // chân trig của HC-SR04
const int echo = 8;     // chân echo của HC-SR04
 
void setup()
{
    Serial.begin(9600);     // giao tiếp Serial với baudrate 9600
    pinMode(trig,OUTPUT);   // chân trig sẽ phát tín hiệu
    pinMode(echo,INPUT);    // chân echo sẽ nhận tín hiệu
}
 
void loop()
{
    unsigned long duration; // biến đo thời gian
    int distance;           // biến lưu khoảng cách
    
    /* Phát xung từ chân trig */
    digitalWrite(trig,0);   // tắt chân trig
    delayMicroseconds(2);
    digitalWrite(trig,1);   // phát xung từ chân trig
    delayMicroseconds(5);   // xung có độ dài 5 microSeconds
    digitalWrite(trig,0);   // tắt chân trig
    
    /* Tính toán thời gian */
    // Đo độ rộng xung HIGH ở chân echo. 
    duration = pulseIn(echo,HIGH);  
    // Tính khoảng cách đến vật.
    distance = int(duration/2/29.412);
    
    /* In kết quả ra Serial Monitor */
    Serial.print(distance);
    Serial.println("cm");
    delay(200);
}
 
Giải thích
 
- duration = pulseIn(echo,1);
Hàm pulseIn() được dùng để đo độ rộng của xung, các bạn có thể xem thêm tại link sau: http://arduino.vn/reference/pulsein. Duration sẽ bằng độ dài xung HIGH ở chân echo (tính theo micro giây).
 
- distance = int(duration/2/29.412);

Thời gian sóng truyền từ cảm biến đến vật sẽ bằng duration/2, sau đó ta chia tiếp cho 29,412 để tính khoảng cách.​

 

Với stm32 và HCR04:

 

Sơ đồ nguyên lý:
Screenshot - 8_15_2014 , 7_08_28 PM. 
Screenshot - 8_15_2014 , 6_56_51 PM. 

I. Cảm biến siêu âm
1. Nguyên lý hoạt động của cảm biến siêu âm:


Screenshot - 8_15_2014 , 6_52_10 PM. ​

Trên cảm biến có 4 chân:


  • VCC: nguồn cấp cho cảm biến 5V
  • GND: đất
  • Trigger: pin cho cực phát ---- nối trực tiếp vào chân của vdk
  • Echo: pin cho cực thu ---- nối trực tiếp vào chân của vdk

Nguyên lý hoạt động:


Screenshot - 8_15_2014 , 6_58_17 PM. ​
  • Khi bắt đầu thì vđk sẽ phát 1 xung vào pin Trigger có độ rộng là 10us
  • Sau khi có xung kích vào chân Trigger thì cực phát của cảm biến sẽ phát ra 8 xung của sóng âm thanh ( v= 340m/s)
  • Ngay sau đó pin Echo ở vdk sẽ đẽ kéo lên cao và đợi sóng âm thanh phản xạ lại. khi gặp sóng phản xạ thì pin Echo sẽ được kéo xuống thấp. Như vậy, thời gian pin Echo ở mức cao sẽ chính là thời gian sóng âm thanh đi và về trên quãng đường từ cảm biến tới vật cản. chúng ta sẽ dùng timer để đếm khoảng thời gian này
Screenshot - 8_15_2014 , 7_03_04 PM. ​

Mẫu code:

Mã:
*Sensor | MCU
_____________
Trig   | PB5
Echo   | PB4 */
 
#include
#include
 
#ifndef HCSR04_H_
#define HCSR04_H_
 
#define US_PORT PORTB
#define    US_PIN    PINB
#define US_DDR     DDRB
 
#define US_TRIG_POS    PB5
#define US_ECHO_POS    PB4
#define US_ERROR        -1
#define    US_NO_OBSTACLE    -2
void HCSR04Init();
void HCSR04Trigger();
 
void HCSR04Init()
{
    US_DDR|=(1<<us_trig_pos);
}
 
void HCSR04Trigger() // Kép Pin Trigger lên cao
{
    //Send a 10uS pulse on trigger line
   
    US_PORT|=(1<
   
    _delay_us(15);                //wait 15uS
   
    US_PORT&=~(1<
}
 
uint16_t GetPulseWidth() // lấy độ rộng xung chân Echo
{
    uint32_t i,result;
 
    //Wait for the rising edge
    for(i=0;i<600000;i++) //counter dem 32bit
    {
        if(!(US_PIN & (1<<us_echo_pos)))
        continue;    //Line is still low, so wait
        else
        break;        //High edge detected, so break.
    }
 
    if(i==600000)
    return US_ERROR;    //Indicates time out
   
    //High Edge Found
 
    //Setup Timer1
 
    TCCR1B=(1<
    TCNT1=0x00;            //Init counter
 
    //Now wait for the falling edge
    for(i=0;i<600000;i++)
    {
        if(US_PIN & (1<<us_echo_pos))
        {
            if(TCNT1 > 60000) break; else continue;            // ( chi lay tu 1--60000)
        }
        else
        break;
    }
 
    if(i==600000)
    return US_NO_OBSTACLE;    //Indicates time out
 
    //Falling edge found
 
    result=TCNT1;
 
    //Stop Timer
    TCCR1B=0x00;
 
    if(result > 60000)
    return US_NO_OBSTACLE;    //No obstacle
    else
    return (result>>1);
}
 
 
 
#endif
/* HCSR04_H_ */
Code cho hàm chính:
Mã:
#define F_CPU 16000000
#include
#include
#include "lcd.h"
#include "HCSR04.h"
#include "led.h"
#include "motor.h"
int main()
{
    uint16_t r;
    _delay_ms(100);    //Let the system start
    // initialize system
    LCDInit(LS_NONE);
    _delay_ms(100);
   
 
    HCSR04Init();
    _delay_ms(50);
   
    GREEN_LED_blink();
    _delay_ms(200);
    YELLOW_LED_blink();
    _delay_ms(200);
    RED_LED_blink();
    _delay_ms(200);
   
    LCDClear();
    LCDWriteStringXY(0,0,"Distance Sensor");
    LCDWriteStringXY(3,1,"CTTT CDT K55");
    _delay_ms(1000);
    LCDClear();
 
 
    while(2)
    {
 
       
        //Send a trigger pulse
        HCSR04Trigger();
 
        //Measure the width of pulse
        r=GetPulseWidth();
 
        //Handle Errors
        if(r==US_ERROR)
        {
            LCDWriteStringXY(0,0,"Error !");
            _delay_ms(200);
            LCDClear();
        }
        else if(r==US_NO_OBSTACLE)
        {
            LCDWriteStringXY(0,0,"Clear !");
            _delay_ms(200);
            LCDClear();
        }
        else
        {
            int d;
            d=(r/58.0);    //Convert to cm (/58 16mhz, /29 8MHz)
            /*We are running on CPU speed of 16MHz, so our i/o clock is
              16MHz. We divide this by 8 to get a 2MHz clock for our timer. This
              means the timer increments its value in 0.5 microsecond. We can
              divide the value by two to get exact 1 microsecond time base.  */
            LCDWriteIntXY(4,0,d,4);
            LCDWriteString(" cm");
            _delay_ms(200);
            if (d>0 && d<=20)
            { 
                LCDWriteStringXY(0,1,"                ");
                LCDWriteStringXY(0,1,"DANGEROUS !");
                control(0);   
                RED_LED_blink();
            }
            if (d>20 && d<=50)
            {
                LCDWriteStringXY(0,1,"                ");
                LCDWriteStringXY(0,1,"WARNING !");
                control(110);
                YELLOW_LED_blink();
            }
            if (d>50)
            {
                    LCDWriteStringXY(0,1,"                ");
                    LCDWriteStringXY(0,1,"SAFING !");
                    control(255);
                    GREEN_LED_blink();
            }
           
        }
    }
 
}
thêm code băm xung động cơ, dùng timer 2 :D
Mã:
void control(int i)
{
    MOTOR_PORT = 0x00;
    MOTOR_DDR |= (1<<motor);
    TCCR2 = (1<<wgm21)|(1<<wgm20)|(1<<com21)|(1<
    OCR2 = i; // i of 255

}

 

 

</wgm21)|(1<<wgm20)|(1<<com21)|(1<
</motor);<></div> </us_echo_pos))<></div> </cs11);></div> </us_echo_pos)))<></div> </us_trig_pos);></div> </us_trig_pos);></div> </us_trig_pos);<></div>
</us_echo_pos))<></div> </cs11);></div> </us_echo_pos)))<></div> </us_trig_pos);></div> </us_trig_pos);></div> </us_trig_pos);<></div>
</us_echo_pos)))<></div> </us_trig_pos);></div> </us_trig_pos);></div> </us_trig_pos);<></div>
</us_trig_pos);<></div>
Đánh giá bài viết
Tổng số điểm của bài viết là: 0 trong 0 đánh giá
Click để đánh giá bài viết
 

Giới thiệu sản phẩm dịch vụ

Giới thiệu sản phẩm dịch vụ: -          Cung cấp giải pháp, thiết kế các sản phẩm Robot gia dụng -          Cung cấp các sản phẩm điện thông minh, nhà thông minh -          Sửa chữa, phục hồi chức năng cho Robot -  ...

Thăm dò ý kiến

Bạn có muốn sở hữu một Robot trong nhà không?

Cần một Robot để dọn dẹp

Cần một Robot trông nh

Cần một Robot để giải trí

Bạn cần một Robot theo cách khác

Bạn đã có rồi

Bạn không cần

Liên kết