arduino_car/car.ino

/* 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;

/* Ultrasonic sensor variables */
long duration;
int distance;

void setup() {
  /* Setup pins */
  /* 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 */

/* 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 */
  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 */
  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 */
void loop() {
  if (get_distance() > 25) {
    move_forward();
    delay(1000);
  } else {
    turn_left(500);
    move_backward();
    delay(1000);
    turn_right(500);
  }
}