Arduino Robot + SR04 example

arduino_robot_sr04.ino

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

/*
 * Arduino Robot + SR04 example
 */

#define echoPin TKD3
#define trigPin TKD4

void setup() {
  // put your setup code here, to run once:
  Robot.begin();
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  detectWall(9);
}

/* We want to detect based on distance (d) in cm */
void detectWall(int targetDistance) {
  long duration, distance;

  Robot.digitalWrite(TKD3, HIGH);

  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;

  //  if (distance < targetDistance) {
  //    obstacleDetected();
  //  } else {
  //    noWall();
  //  }

  //  if (distance >= 200 || distance <= 0) {
  //    Serial.println("Out of range");
  //  }
  //  else {
  //    logDistanceToSerial(distance);
  //  }

  logDistanceToSerial(distance);
  delay(500);
}

/* What to do when wall is detected? */
void obstacleDetected() {
  Serial.println("Detected");
}

void noWall() {
  Serial.println("No wall");
}

void logDistanceToSerial(int distance) {
  Serial.print(distance);
  Serial.println(" cm");
}