ino.cpp

// IDE
#ifndef LOW
#define LOW 0
#endif

#ifndef HIGH
#define HIGH 255
#endif


// Iron Settings

#define LMOTOR 6
#define RMOTOR 5

#define LDIR 7
#define RDIR 4

#define LSENS 3
#define RSENS 4
#define CSENS 5

// false - left, true - right
//#define REV false

#define SPD 100

#define VCC 5.0


#include "Ultrasonic.h"

// sensor connected to:
// Trig - 12, Echo - 13
// Trig - 10, Echo - 11
Ultrasonic LSensor(12, 13);
Ultrasonic RSensor(10, 11);


float
        left_p = 150,
        right_p = 150;

void setup() {

    Serial.begin(9600);

    //    init iron
    pinMode(LMOTOR, OUTPUT);
    pinMode(RMOTOR, OUTPUT);
    pinMode(LDIR, OUTPUT);
    pinMode(RDIR, OUTPUT);


    //    all on, captain
    digitalWrite(LDIR, HIGH);
    analogWrite(LMOTOR, SPD);
    analogWrite(RMOTOR, SPD);


}

void loop() {

    // get distance
    float
            left_d = LSensor.Ranging(CM),
            right_d = RSensor.Ranging(CM);

    // avrg distance and remember it
    float
            left = (left_d + left_p) / 2,
            right = (right_d + right_p) / 2;

    left_p = left_d;
    right_p = right_d;


    Serial.print(left);
    Serial.print(" :: ");
    Serial.println(right);


//    if (right <= 15 && left <=/ 15 && left > 10 && right > 10) {

//      digitalWrite(LDIR, LOW);
//      digitalWrite(RDIR, LOW);

//    } else {

        digitalWrite(LDIR, right <= 12 ? LOW : HIGH);
        digitalWrite(RDIR, left <= 12 ? HIGH : LOW);

//    }


    delay(50);

}

Ultrasonic.cpp

/*
  Ultrasonic.cpp - Library for HC-SR04 Ultrasonic Ranging Module.library

  Created by ITead studio. Apr 20, 2010.
  iteadstudio.com
  
  updated by noonv. Feb, 2011
  http://robocraft.ru
*/

#include "Ultrasonic.h"

Ultrasonic::Ultrasonic(int TP, int EP) {
    pinMode(TP, OUTPUT);
    pinMode(EP, INPUT);
    Trig_pin = TP;
    Echo_pin = EP;
}

long Ultrasonic::Timing() {
    digitalWrite(Trig_pin, LOW);
    delayMicroseconds(2);
    digitalWrite(Trig_pin, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trig_pin, LOW);
    duration = pulseIn(Echo_pin, HIGH);
    return duration;
}

long Ultrasonic::Ranging(int sys) {
    Timing();
    distacne_cm = duration / 29 / 2;
    distance_inc = duration / 74 / 2;
    if (sys)
        return distacne_cm;
    else
        return distance_inc;
}

Ultrasonic.h

/*
  Ultrasonic.h - Library for HR-SC04 Ultrasonic Ranging Module.
  Created by ITead studio. Alex, Apr 20, 2010.
  iteadstudio.com
  
  updated by noonv. Feb, 2011
  http://robocraft.ru
*/

#ifndef Ultrasonic_h
#define Ultrasonic_h

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else

#include "WProgram.h"

#endif

#define CM 1
#define INC 0

class Ultrasonic {
public:
    Ultrasonic(int TP, int EP);

    long Timing();

    long Ranging(int sys);

private:
    int Trig_pin;
    int Echo_pin;
    long duration, distacne_cm, distance_inc;

};

#endif //#ifndef Ultrasonic_h