Modified Arduino Ping))) example to work with 4-Pin HC-SR04 Ultrasonic Sensor Distance Measuring Module

hc-sr04.ino

/* HC-SR04 Sensor
   https://www.dealextreme.com/p/hc-sr04-ultrasonic-sensor-distance-measuring-module-133696
  
   This sketch reads a HC-SR04 ultrasonic rangefinder and returns the
   distance to the closest object in range. To do this, it sends a pulse
   to the sensor to initiate a reading, then listens for a pulse 
   to return.  The length of the returning pulse is proportional to 
   the distance of the object from the sensor.
     
   The circuit:
	* VCC connection of the sensor attached to +5V
	* GND connection of the sensor attached to ground
	* TRIG connection of the sensor attached to digital pin 2
	* ECHO connection of the sensor attached to digital pin 4


   Original code for Ping))) example was created by David A. Mellis
   Adapted for HC-SR04 by Tautvidas Sipavicius

   This example code is in the public domain.
 */


const int trigPin = 2;
const int echoPin = 4;

void setup() {
  // initialize serial communication:
  Serial.begin(9600);
}

void loop()
{
  // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:
  long duration, inches, cm;

  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(trigPin, OUTPUT);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);

  // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  
  delay(100);
}

long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

Works great, thanks!

Is there a reason to not define pinMode in setup()?

Do you have intermittent freezing with this code? I'm trying to set mine up and it seems to work most of the time but occasionally will freeze for a second or so. Any ideas what it might be?

Thank you for the code. I used it in one of my videos: https://www.youtube.com/watch?v=03scimJP-oI

I hope that's not a problem!

To JoeWoodward this is because something is to close, its an "Error" that was made to show in the code.

**not made to show in the code

I am using this code for a project. Works pretty well.

nice work man

A thing of beauty! Thanks for the contribution.

works correctly,.......

but i want linked program for GSM modem, i mean the distance in cm is go through the GSM modem to mobile... in sms form.

can I use this to use 2 hsr04 at the same time?

Thanks, it worked great!

Nice work thanks

how to determine the Serial.begin() of the board?

Nice Job! Thanks

Awsome work! Thanks

How can both codes be joined?

#include <ZumoBuzzer.h>
#include <ZumoMotors.h>
#include <Pushbutton.h>
#include <QTRSensors.h>
#include <ZumoReflectanceSensorArray.h>
#include <avr/pgmspace.h>
#include <Wire.h>
#include <LSM303.h>

/* This example uses the accelerometer in the Zumo Shield's onboard LSM303DLHC with the LSM303 Library to

• detect contact with an adversary robot in the sumo ring. The LSM303 Library is not included in the Zumo
• Shield libraries; it can be downloaded separately from GitHub at:
• https://github.com/pololu/LSM303
• This example extends the BorderDetect example, which makes use of the onboard Zumo Reflectance Sensor Array
• and its associated library to detect the border of the sumo ring. It also illustrates the use of the
• ZumoMotors, PushButton, and ZumoBuzzer libraries.
• In loop(), the program reads the x and y components of acceleration (ignoring z), and detects a
• contact when the magnitude of the 3-period average of the x-y vector exceeds an empirically determined
• XY_ACCELERATION_THRESHOLD. On contact detection, the forward speed is increased to FULL_SPEED from
• the default SEARCH_SPEED, simulating a "fight or flight" response.
• The program attempts to detect contact only when the Zumo is going straight. When it is executing a
• turn at the sumo ring border, the turn itself generates an acceleration in the x-y plane, so the
• acceleration reading at that time is difficult to interpret for contact detection. Since the Zumo also
• accelerates forward out of a turn, the acceleration readings are also ignored for MIN_DELAY_AFTER_TURN
• milliseconds after completing a turn. To further avoid false positives, a MIN_DELAY_BETWEEN_CONTACTS is
• also specified.
• This example also contains the following enhancements:
• • uses the Zumo Buzzer library to play a sound effect ("charge" melody) at start of competition and
• whenever contact is made with an opposing robot
• • randomizes the turn angle on border detection, so that the Zumo executes a more effective search pattern
• • supports a FULL_SPEED_DURATION_LIMIT, allowing the robot to switch to a SUSTAINED_SPEED after a short
• period of forward movement at FULL_SPEED. In the example, both speeds are set to 400 (max), but this
• feature may be useful to prevent runoffs at the turns if the sumo ring surface is unusually smooth.
• • logging of accelerometer output to the serial monitor when LOG_SERIAL is #defined.
• This example also makes use of the public domain RunningAverage library from the Arduino website; the relevant
• code has been copied into this .ino file and does not need to be downloaded separately.
  */

// #define LOG_SERIAL // write log output to serial port

#define LED 13
Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12

// Accelerometer Settings
#define RA_SIZE 3 // number of readings to include in running average of accelerometer readings
#define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity)

// Reflectance Sensor Settings
#define NUM_SENSORS 6
unsigned int sensor_values[NUM_SENSORS];
// this might need to be tuned for different lighting conditions, surfaces, etc.
#define QTR_THRESHOLD 1400 // microseconds
ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN);

// Motor Settings
ZumoMotors motors;

// these might need to be tuned for different motor types
#define REVERSE_SPEED 400 // 0 is stopped, 400 is full speed
#define TURN_SPEED 400
#define SEARCH_SPEED 400
#define SUSTAINED_SPEED 400 // switches to SUSTAINED_SPEED from FULL_SPEED after FULL_SPEED_DURATION_LIMIT ms
#define FULL_SPEED 400
#define STOP_DURATION 100 // ms
#define REVERSE_DURATION 200 // ms
#define TURN_DURATION 400 // ms

#define RIGHT 1
#define LEFT -1

enum ForwardSpeed { SearchSpeed, SustainedSpeed, FullSpeed };
ForwardSpeed _forwardSpeed; // current forward speed setting
unsigned long full_speed_start_time;
#define FULL_SPEED_DURATION_LIMIT 250 // ms

// Sound Effects
ZumoBuzzer buzzer;
const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // "charge" melody
// use V0 to suppress sound effect; v15 for max volume

// Timing
unsigned long loop_start_time;
unsigned long last_turn_time;
unsigned long contact_made_time;
#define MIN_DELAY_AFTER_TURN 400 // ms = min delay before detecting contact event
#define MIN_DELAY_BETWEEN_CONTACTS 1000 // ms = min delay between detecting new contact event

// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
template
class RunningAverage
{
public:
RunningAverage(void);
RunningAverage(int);
~RunningAverage();
void clear();
void addValue(T);
T getAverage() const;
void fillValue(T, int);
protected:
int _size;
int _cnt;
int _idx;
T _sum;
T * _ar;
static T zero;
};

// Accelerometer Class -- extends the LSM303 Library to support reading and averaging the x-y acceleration
// vectors from the onboard LSM303DLHC accelerometer/magnetometer
class Accelerometer : public LSM303
{
typedef struct acc_data_xy
{
unsigned long timestamp;
int x;
int y;
float dir;
} acc_data_xy;

public:
Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {};
~Accelerometer() {};
void enable(void);
void getLogHeader(void);
void readAcceleration(unsigned long timestamp);
float len_xy() const;
float dir_xy() const;
int x_avg(void) const;
int y_avg(void) const;
long ss_xy_avg(void) const;
float dir_xy_avg(void) const;
private:
acc_data_xy last;
RunningAverage ra_x;
RunningAverage ra_y;
};

Accelerometer lsm303;
boolean in_contact; // set when accelerometer detects contact with opposing robot

// forward declaration
void setForwardSpeed(ForwardSpeed speed);

void setup()
{
// Initiate the Wire library and join the I2C bus as a master
Wire.begin();

// Initiate LSM303
lsm303.init();
lsm303.enable();

#ifdef LOG_SERIAL
Serial.begin(9600);
lsm303.getLogHeader();
#endif

randomSeed((unsigned int) millis());

// uncomment if necessary to correct motor directions
motors.flipLeftMotor(true);
motors.flipRightMotor(true);

pinMode(LED, HIGH);
buzzer.playMode(PLAY_AUTOMATIC);
waitForButtonAndCountDown(false);
}

void waitForButtonAndCountDown(bool restarting)
{
#ifdef LOG_SERIAL
Serial.print(restarting ? "Restarting Countdown" : "Starting Countdown");
Serial.println();
#endif

digitalWrite(LED, HIGH);
button.waitForButton();
digitalWrite(LED, LOW);

// play audible countdown
for (int i = 0; i < 3; i++)
{
delay(1000);
buzzer.playNote(NOTE_G(3), 50, 12);
}
delay(1000);
buzzer.playFromProgramSpace(sound_effect);
delay(1000);

// reset loop variables
in_contact = false; // 1 if contact made; 0 if no contact or contact lost
contact_made_time = 0;
last_turn_time = millis(); // prevents false contact detection on initial acceleration
_forwardSpeed = SearchSpeed;
full_speed_start_time = 0;
}

void loop()
{

if (button.isPressed())
{
// if button is pressed, stop and wait for another press to go again
motors.setSpeeds(0, 0);
button.waitForRelease();
waitForButtonAndCountDown(true);
}

loop_start_time = millis();
lsm303.readAcceleration(loop_start_time);
sensors.read(sensor_values);

if ((_forwardSpeed == FullSpeed) && (loop_start_time - full_speed_start_time > FULL_SPEED_DURATION_LIMIT) && (cm > 0) )
{
setForwardSpeed(SustainedSpeed);
}

if (sensor_values[0] < QTR_THRESHOLD)
{
// if leftmost sensor detects line, reverse and turn to the right
turn(RIGHT, true);
}
else if (sensor_values[5] < QTR_THRESHOLD)
{
// if rightmost sensor detects line, reverse and turn to the left
turn(LEFT, true);
}
else // otherwise, go straight
{
if (check_for_contact()) on_contact_made();
int speed = getForwardSpeed();
motors.setSpeeds(speed, speed);
}
}

// execute turn
// direction: RIGHT or LEFT
// randomize: to improve searching
void turn(char direction, bool randomize)
{
#ifdef LOG_SERIAL
Serial.print("turning ...");
Serial.println();
#endif

// assume contact lost
on_contact_lost();

static unsigned int duration_increment = TURN_DURATION / 4;

motors.setSpeeds(0,0);
delay(STOP_DURATION);
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(TURN_SPEED * direction, -TURN_SPEED * direction);
delay(randomize ? TURN_DURATION + (random(8) - 2) * duration_increment : TURN_DURATION);
int speed = getForwardSpeed();
motors.setSpeeds(speed, speed);
last_turn_time = millis();
}

void setForwardSpeed(ForwardSpeed speed)
{
_forwardSpeed = speed;
if (speed == FullSpeed) full_speed_start_time = loop_start_time;
}

int getForwardSpeed()
{
int speed;
switch (_forwardSpeed)
{
case FullSpeed:
speed = FULL_SPEED;
break;
case SustainedSpeed:
speed = SUSTAINED_SPEED;
break;
default:
speed = SEARCH_SPEED;
break;
}
return speed;
}

// check for contact, but ignore readings immediately after turning or losing contact
bool check_for_contact()
{
static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
return (lsm303.ss_xy_avg() > threshold_squared) &&
(loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN) &&
(loop_start_time - contact_made_time > MIN_DELAY_BETWEEN_CONTACTS);
}

// sound horn and accelerate on contact -- fight or flight
void on_contact_made()
{
#ifdef LOG_SERIAL
Serial.print("contact made");
Serial.println();
#endif
in_contact = true;
contact_made_time = loop_start_time;
setForwardSpeed(FullSpeed);
buzzer.playFromProgramSpace(sound_effect);
}

// reset forward speed
void on_contact_lost()
{
#ifdef LOG_SERIAL
Serial.print("contact lost");
Serial.println();
#endif
in_contact = false;
setForwardSpeed(SearchSpeed);
}

// class Accelerometer -- member function definitions

// enable accelerometer only
// to enable both accelerometer and magnetometer, call enableDefault() instead
void Accelerometer::enable(void)
{
// Enable Accelerometer
// 0x27 = 0b00100111
// Normal power mode, all axes enabled
writeAccReg(LSM303::CTRL_REG1_A, 0x27);

if (getDeviceType() == LSM303::device_DLHC)
writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
}

void Accelerometer::getLogHeader(void)
{
Serial.print("millis x y len dir | len_avg dir_avg | avg_len");
Serial.println();
}

void Accelerometer::readAcceleration(unsigned long timestamp)
{
readAcc();
if (a.x == last.x && a.y == last.y) return;

last.timestamp = timestamp;
last.x = a.x;
last.y = a.y;

ra_x.addValue(last.x);
ra_y.addValue(last.y);

#ifdef LOG_SERIAL
Serial.print(last.timestamp);
Serial.print(" ");
Serial.print(last.x);
Serial.print(" ");
Serial.print(last.y);
Serial.print(" ");
Serial.print(len_xy());
Serial.print(" ");
Serial.print(dir_xy());
Serial.print(" | ");
Serial.print(sqrt(static_cast(ss_xy_avg())));
Serial.print(" ");
Serial.print(dir_xy_avg());
Serial.println();
#endif
}

float Accelerometer::len_xy() const
{
return sqrt(last.xa.x + last.ya.y);
}

float Accelerometer::dir_xy() const
{
return atan2(last.x, last.y) * 180.0 / M_PI;
}

int Accelerometer::x_avg(void) const
{
return ra_x.getAverage();
}

int Accelerometer::y_avg(void) const
{
return ra_y.getAverage();
}

long Accelerometer::ss_xy_avg(void) const
{
long x_avg_long = static_cast(x_avg());
long y_avg_long = static_cast(y_avg());
return x_avg_longx_avg_long + y_avg_longy_avg_long;
}

float Accelerometer::dir_xy_avg(void) const
{
return atan2(static_cast(x_avg()), static_cast(y_avg())) * 180.0 / M_PI;
}

// RunningAverage class
// based on RunningAverage library for Arduino
// source: http://playground.arduino.cc/Main/RunningAverage
// author: Rob.Tillart@gmail.com
// Released to the public domain

template
T RunningAverage::zero = static_cast(0);

template
RunningAverage::RunningAverage(int n)
{
_size = n;
_ar = (T*) malloc(_size * sizeof(T));
clear();
}

template
RunningAverage::~RunningAverage()
{
free(_ar);
}

// resets all counters
template
void RunningAverage::clear()
{
_cnt = 0;
_idx = 0;
_sum = zero;
for (int i = 0; i< _size; i++) _ar[i] = zero; // needed to keep addValue simple
}

// adds a new value to the data-set
template
void RunningAverage::addValue(T f)
{
_sum -= _ar[_idx];
_ar[_idx] = f;
_sum += _ar[_idx];
_idx++;
if (_idx == _size) _idx = 0; // faster than %
if (_cnt < _size) _cnt++;
}

// returns the average of the data-set added so far
template
T RunningAverage::getAverage() const
{
if (_cnt == 0) return zero; // NaN ? math.h
return _sum / _cnt;
}

// fill the average with a value
// the param number determines how often value is added (weight)
// number should preferably be between 1 and size
template
void RunningAverage::fillValue(T value, int number)
{
clear();
for (int i = 0; i < number; i++)
{
addValue(value);
}
}