Basic tests for a small robotic car

robotic-car-test.ino

/*
 * robotic-car-test.ino: Basic tests for a small robotic car.
 *
 * Usage: keep the car in your hand and look at the serial monitor.
 */

#include <Servo.h>

// Pinout.
const int MOTOR_LEFT_ENABLE    =  6;
const int MOTOR_LEFT_FORWARD   =  5;
const int MOTOR_LEFT_BACKWARD  =  7;
const int MOTOR_RIGHT_ENABLE   =  9;
const int MOTOR_RIGHT_FORWARD  =  8;
const int MOTOR_RIGHT_BACKWARD = 10;
const int SONAR_TRIGGER        =  2;
const int SONAR_ECHO           =  4;
const int SERVO                =  3;

// Time/distance conversion for the sonar.
const float CENTIMETER = 58.3;  // microseconds

// The car's cute head.
Servo head_servo;

// Return obstacle distance in centimeters.
float distance() {
    digitalWrite(SONAR_TRIGGER, HIGH);
    delayMicroseconds(10);
    digitalWrite(SONAR_TRIGGER, LOW);
    return pulseIn(SONAR_ECHO, HIGH) / CENTIMETER;
}

void setup() {
    pinMode(MOTOR_LEFT_ENABLE,    OUTPUT);
    pinMode(MOTOR_LEFT_FORWARD,   OUTPUT);
    pinMode(MOTOR_LEFT_BACKWARD,  OUTPUT);
    pinMode(MOTOR_RIGHT_ENABLE,   OUTPUT);
    pinMode(MOTOR_RIGHT_FORWARD,  OUTPUT);
    pinMode(MOTOR_RIGHT_BACKWARD, OUTPUT);
    pinMode(SONAR_TRIGGER,        OUTPUT);
    pinMode(SONAR_ECHO,           INPUT);
    head_servo.attach(SERVO);
    Serial.begin(9600);
}

void loop() {

    /* Test the motors. */

    Serial.println();
    Serial.println(F("Forward 25%"));
    digitalWrite(MOTOR_LEFT_FORWARD,   HIGH);
    digitalWrite(MOTOR_LEFT_BACKWARD,  LOW);
    digitalWrite(MOTOR_RIGHT_FORWARD,  HIGH);
    digitalWrite(MOTOR_RIGHT_BACKWARD, LOW);
    analogWrite(MOTOR_LEFT_ENABLE,  63);
    analogWrite(MOTOR_RIGHT_ENABLE, 63);
    delay(3000);

    Serial.println(F("Forward 50%"));
    analogWrite(MOTOR_LEFT_ENABLE,  127);
    analogWrite(MOTOR_RIGHT_ENABLE, 127);
    delay(3000);

    Serial.println(F("Forward 100%"));
    analogWrite(MOTOR_LEFT_ENABLE,  255);
    analogWrite(MOTOR_RIGHT_ENABLE, 255);
    delay(3000);

    Serial.println(F("Coast"));
    digitalWrite(MOTOR_LEFT_ENABLE,  LOW);
    digitalWrite(MOTOR_RIGHT_ENABLE, LOW);
    delay(1000);

    Serial.println(F("Break"));
    digitalWrite(MOTOR_LEFT_FORWARD,  LOW);
    digitalWrite(MOTOR_RIGHT_FORWARD, LOW);
    analogWrite(MOTOR_LEFT_ENABLE,  255);
    analogWrite(MOTOR_RIGHT_ENABLE, 255);
    delay(1000);

    Serial.println(F("Reverse"));
    digitalWrite(MOTOR_LEFT_BACKWARD,  HIGH);
    digitalWrite(MOTOR_RIGHT_BACKWARD, HIGH);
    analogWrite(MOTOR_LEFT_ENABLE,  63);
    analogWrite(MOTOR_RIGHT_ENABLE, 63);
    delay(3000);

    Serial.println(F("Break"));
    digitalWrite(MOTOR_LEFT_BACKWARD,  LOW);
    digitalWrite(MOTOR_RIGHT_BACKWARD, LOW);
    analogWrite(MOTOR_LEFT_ENABLE,  255);
    analogWrite(MOTOR_RIGHT_ENABLE, 255);
    delay(1000);

    /* Test the head servo and the sonar. */

    Serial.println();
    Serial.print(F("Looking left:     "));
    head_servo.write(120);
    delay(1000);
    Serial.print(F("obstacle distance = "));
    Serial.print(distance());
    Serial.println(F(" cm"));
    delay(1000);

    Serial.print(F("Looking straight: "));
    head_servo.write(90);
    delay(1000);
    Serial.print(F("obstacle distance = "));
    Serial.print(distance());
    Serial.println(F(" cm"));
    delay(1000);

    Serial.print(F("Looking right:    "));
    head_servo.write(60);
    delay(1000);
    Serial.print(F("obstacle distance = "));
    Serial.print(distance());
    Serial.println(F(" cm"));
    delay(1000);
}