Ultrasonic range finder

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;
        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
        case 1:
            address = 0xE0;
        case 2: 
            address = 0xE2;
        case 3: 
            address = 0xE4;
    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
    address = 0xE2; //left
    address = 0xE4; //mid
    // 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



You need to log in to post a comment