Arduino Robot + SR04 x 3 example

arduino_robot_sr04_3_sensors.ino

#include <ArduinoRobot.h>
#include <SPI.h>
#include "Wire.h"

/*
 * Arduino Robot + SR04 example
 */

// sensor1
#define echoPin1 TKD1
#define trigPin1 TKD3

// sensor2
#define echoPin2 TKD4
#define trigPin2 TKD5

// sensor3
#define echoPin3 TKD0
#define trigPin3 TKD2

void setup() {
  // put your setup code here, to run once:
  Robot.begin();
  Serial.begin(9600);
  pinMode(echoPin1, INPUT);
  pinMode(echoPin2, INPUT);
  pinMode(echoPin3, INPUT);

  pinMode(trigPin1, OUTPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(trigPin3, OUTPUT);
}

void loop() {
  sense(1, echoPin1, trigPin1);
  sense(2, echoPin2, trigPin2);
  sense(3, echoPin3, trigPin3);
}

/* Code that runs the sensor */
void sense(int sensorNum, int echoPin, int trigPin) {
  long duration, distance;
  Robot.digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  Robot.digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); // Added this line
  Robot.digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;

  delay(500);
  logDistanceToSerial(sensorNum, distance);
}

// Display distance and sensor number
void logDistanceToSerial(int sensorNum, int distance) {
  Serial.print("Sensor: ");
  Serial.print(sensorNum);
  Serial.print(" ");
  Serial.print(distance);
  Serial.println(" cm");
}