Use the sensor library

car.ino

@@ -1,3 +1,5 @@
+#include <hcsr04.h>
+
 /* Define your pins here */
 /* Wheels */
 const int left_wheel_1  = 0;
@@ -9,8 +11,7 @@ const int right_wheel_2 = 3;
 const int sensor_echo    = 9;
 const int sensor_trigger = 10;
 
-/* Debug serial output */
+HCSR04 hcsr04(sensor_trigger, sensor_echo, 20, 4000);
-bool debug = true;
 
 /* Array bellow contains all the wheels */
 int wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};
@@ -18,6 +19,8 @@ int wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};
 /* The counter */
 int i;
 
+int mm;
+
 /* Ultrasonic sensor variables */
 long duration;
 int distance;
@@ -41,108 +44,75 @@ void setup() {
 
 /* Forward */
 void move_forward_wheel_left() {
-  if (debug) { debug_log("WH", "Move forward left wheel"); }
   digitalWrite(left_wheel_1, HIGH);
   digitalWrite(left_wheel_2, LOW);
 }
 
 void move_forward_wheel_right() {
-  if (debug) { debug_log("WH", "Move forward right wheel"); }
   digitalWrite(right_wheel_1, HIGH);
   digitalWrite(right_wheel_2, LOW);
 }
 
 void move_forward() {
-  if (debug) { debug_log("WH", "Move forward"); }
   move_forward_wheel_left();
   move_forward_wheel_right();
 }
 
 /* Backward */
 void move_backward_wheel_left() {
-  if (debug) { debug_log("WH", "Move backward left wheel"); }
   digitalWrite(left_wheel_1, LOW);
   digitalWrite(left_wheel_2, HIGH);
 }
 
 void move_backward_wheel_right() {
-  if (debug) { debug_log("WH", "Move backward right wheel"); }
   digitalWrite(right_wheel_1, LOW);
   digitalWrite(right_wheel_2, HIGH);
 }
 
 void move_backward() {
-  if (debug) { debug_log("WH", "Move backward"); }
   move_backward_wheel_left();
   move_backward_wheel_right();
 }
 
 /* Stops */
 void stop_all() {
-  if (debug) { debug_log("WH", "Stop the wheels"); }
   for (i = 0; sizeof(wheels) - 1; i++) {
     digitalWrite(wheels[i], LOW);
   }
 }
 
 void stop_left() {
-  if (debug) { debug_log("WH", "Stop left wheel"); }
   digitalWrite(left_wheel_1, LOW);
   digitalWrite(left_wheel_2, LOW);
 }
 
 void stop_right() {
-  if (debug) { debug_log("WH", "Stop right wheel"); }
   digitalWrite(right_wheel_1, LOW);
   digitalWrite(right_wheel_2, LOW);
 }
 
 /* Turns */
 void turn_left(int delayMS) {
-  if (debug) { debug_log("WH", "Turn left"); }
   stop_left();
   move_forward_wheel_right();
   delay(delayMS);
 }
 
 void turn_right(int delayMS) {
-  if (debug) { debug_log("WH", "Turn right"); }
   stop_right();
   move_forward_wheel_left();
   delay(delayMS);
 }
 
-/* Helpers */
-void debug_log(char tag, char message) {
-  Serial.print("[");
-  Serial.print(tag);
-  Serial.print("] ");
-  Serial.println(message);
-}
-
 /* Ultrasonic sensor actions */
 
 /* get_distance -- returns distance to an object in centimeters */
 int get_distance() {
-  /* Clears the trigger */
+  mm = hcsr04.distanceInMillimeters();
-  if (debug) { debug_log("US", "Clear the trigger"); }
+  if (mm > 0) {
-  digitalWrite(sensor_trigger, LOW);
+    return mm / 10;
-  delayMicroseconds(2);
+  }
-
+  return 0;
-  /* Set the trigger on HIGH state for 10 microseconds */
-  if (debug) { debug_log("US", "Set trigger HIGH"); }
-  digitalWrite(sensor_trigger, HIGH);
-  delayMicroseconds(10);
-  digitalWrite(sensor_trigger, LOW);
-
-  /* Reads the echo, returns the sound wave travel in microseconds */
-  duration = pulseIn(sensor_echo, HIGH);
-  if (debug) { debug_log("US", "Get echo duration"); }
-
-  /* Calculating the distance */
-  distance = duration / 58;
-  if (debug) { debug_log("US", "Got distance!"); }
-  return distance;
 }
 
 /* Main code */