Ultrasonic sensor

car.ino

@@ -1,20 +1,40 @@
-/* Define you pins here */
+/* Define your pins here */
+/* Wheels */
 const int left_wheel_1  = 0;
 const int left_wheel_2  = 1;
 const int right_wheel_1 = 2;
 const int right_wheel_2 = 3;
 
+/* Ultrasonic sensor */
+const int sensor_echo    = 9;
+const int sensor_trigger = 10;
+
+/* Debug serial output */
+bool debug = true;
+
+/* Array bellow contains all the wheels */
+int wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};
+
 /* The counter */
 int i;
 
-/* Array bellow contains all the wheels */
-int Wheels[] = {left_wheel_1, left_wheel_2, right_wheel_1, right_wheel_2};
+/* Ultrasonic sensor variables */
+long duration;
+int distance;
 
 void setup() {
   /* Setup pins */
-  for (i = 0; i < sizeof(Wheels) - 1; i++) {
-    pinMode(Wheels[i], OUTPUT);
+  /* wheels */
+  for (i = 0; i < sizeof(wheels) - 1; i++) {
+    pinMode(wheels[i], OUTPUT);
   }
+
+  /* Ultrasonic sensor */
+  pinMode(sensor_echo, INPUT);
+  pinMode(sensor_trigger, OUTPUT);
+
+  /* Starts serial communication */
+  Serial.begin(9600);
 }
 
 /* Wheel actions */
@@ -53,8 +73,8 @@ void move_backward() {
 
 /* Stops */
 void stop_all() {
-  for (i = 0; sizeof(Wheels) - 1; i++) {
-    digitalWrite(Wheels[i], LOW);
+  for (i = 0; sizeof(wheels) - 1; i++) {
+    digitalWrite(wheels[i], LOW);
   }
 }
 
@@ -81,18 +101,50 @@ void turn_right(int delayMS) {
   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 */
+  if (debug) { debug_log("US", "Clear the trigger"); }
+  digitalWrite(sensor_trigger, LOW);
+  delayMicroseconds(2);
+
+  /* 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 */
+  if (debug) { debug_log("US", "Get echo duration"); }
+  duration = pulseIn(sensor_echo, HIGH);
+
+  /* Calculating the distance */
+  distance = duration / 58;
+  if (debug) {
+    debug_log("US", "Got distance!");
+  }
+  return distance;
+}
+
 /* Main code */
 void loop() {
+  if (get_distance() > 25) {
     move_forward();
     delay(1000);
+  } else {
     turn_left(500);
     move_backward();
     delay(1000);
     turn_right(500);
-  move_forward();
-  turn_left(500);
-  turn_left(500);
-  turn_left(500);
-  move_forward();
-  delay(1000);
+  }
 }