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;
}

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: [email protected]
// 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);
}
}