Ultrasonic range finder

Page last updated 03 May 2011, by   Alex Louden   distance, range, sonar, SRF02, ultrasonic | 0 replies  

I can't believe it - I wrote the code, plugged it in, and it worked first try.

I also separated my main.cpp file into accelerometer.cpp, dist.cpp, main.cpp, and main.h.

After a few simple tests, it looks like the sensor is accurate to within 1-2% - excessive for avoiding obstacles.

(Distance from sensor to my ceiling is around 165 cms)

Single sensor:

Multiple sensors:

(They were too close together, the 3rd one reflected into the 2nd, so it didn't register an obstacle)

#include "main.h"

//----------------- distance sensors -----------------

I2C dist_i2c(p9, p10);

int address = 0xE0;

const int CTRL_REG = 0x00;
const int RANGE_H = 0x02;
const int RANGE_L = 0x03;

bool dist_write_reg(int regno, int value) { // write value into register regno, return success

    char data[2] = {regno, value}; 
    
    return dist_i2c.write(address, data, 2) == 0;
    
}


bool dist_read_reg(int regno, int *value) { // read value from register regno, return success

    char data = regno; 
    
    if (dist_i2c.write(address, &data, 1) == 0 && dist_i2c.read(address, &data, 1) == 0){
        *value = data;
        return true;
    }
    return false;
    
}

int read_dist_reg(){

    int low, high;

    if ( dist_read_reg(RANGE_H, &high) && dist_read_reg(RANGE_L, &low) )
        return (high << 8) | low;
    else
        return -1;

}

void start(){ 
    
    dist_write_reg(CTRL_REG, 0x51); //burst, range results in cms
    
}

void burst(){

    dist_write_reg(CTRL_REG, 0x5C);  // do a burst

}

void start_noburst(){

    dist_write_reg(CTRL_REG, 0x57);  // start ranging, no burst
    
}

int dist_get(int num){ 
    
    //set which sensor to read
    
    switch(num){
        case 1:
            address = 0xE0;
            break;
        case 2: 
            address = 0xE2;
            break;
        case 3: 
            address = 0xE4;
        default: 
            break;
    }
    
    start(); //do burst, start ranging
    
    wait(0.07); //wait 70ms
    
    return read_dist_reg(); //return register value
    
}

dist dist_get_all(){ 
    
    dist dist = {-1,-1,-1}; //create array
    
    // start each sensor
    address = 0xE0; //right
        start_noburst();
    address = 0xE2; //left
        start_noburst();
    address = 0xE4; //mid
        start();
    
    wait(0.07);
    
    // store register values to array
    address = 0xE0;  
    dist.right = read_dist_reg();
    address = 0xE2;
    dist.left = read_dist_reg();
    address = 0xE4;
    dist.mid = read_dist_reg();
    
    return dist; //return array
    
}

void dist_regset(){
/*
    dist_write_reg(CTRL_REG, 0xA0); // set address sequence
    dist_write_reg(CTRL_REG, 0xAA);
    dist_write_reg(CTRL_REG, 0xA5);
    dist_write_reg(CTRL_REG, 0xE4); //E4
*/

}

0 comments

Please log in to post a comment.