How to Build a Robot at Home

#include <Servo.h>

Servo leftMotor;
Servo rightMotor;

const int serialPeriod = 250;       // this limits output to the console to once every 1/4 second
unsigned long timeSerialDelay = 0;

const int loopPeriod = 20;          // this sets how often the sensor takes a reading to 20ms, which is a frequency of 50Hz
unsigned long timeLoopDelay   = 0;

// this assigns the TRIG and ECHO functions to the pins on the Arduino. Make adjustments to the numbers here if you connected differently
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;

int ultrasonic2Distance;
int ultrasonic2Duration;

// this defines the two possible states for the robot: driving forward or turning left
#define DRIVE_FORWARD     0
#define TURN_LEFT         1

int state = DRIVE_FORWARD; // 0 = drive forward (DEFAULT), 1 = turn left

void setup()
{
    Serial.begin(9600);
  
    // these sensor pin configurations
    pinMode(ultrasonic2TrigPin, OUTPUT);
    pinMode(ultrasonic2EchoPin, INPUT);
    
    // this assigns the motors to the Arduino pins
    leftMotor.attach(12);
    rightMotor.attach(13);
}


void loop()
{
    if(digitalRead(2) == HIGH) // this detects the kill switch
    {
        while(1)
        {
            leftMotor.write(90);
            rightMotor.write(90);
        }
    }

    debugOutput(); // this prints debugging messages to the serial console
    
    if(millis() - timeLoopDelay >= loopPeriod)
    {
        readUltrasonicSensors(); // this instructs the sensor to read and store the measured distances
        
        stateMachine();
        
        timeLoopDelay = millis();
    }
}


void stateMachine()
{
    if(state == DRIVE_FORWARD) // if no obstacles detected
    {
        if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of the robot. ultrasonicDistance will be negative for some ultrasonics if there is no obstacle
        {
            // drive forward
            rightMotor.write(180);
            leftMotor.write(0);
        }
        else //  if there's an object in front of us
        {
            state = TURN_LEFT;
        }
    }
    else if(state == TURN_LEFT) // if an obstacle is detected, turn left
    {
        unsigned long timeToTurnLeft = 500; // it takes around .5 seconds to turn 90 degrees. You may need to adjust this if your wheels are a different size than the example
        
        unsigned long turnStartTime = millis(); // save the time that we started turning

        while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until timeToTurnLeft has elapsed
        {
            // turn left, remember that when both are set to "180" it will turn.
            rightMotor.write(180);
            leftMotor.write(180);
        }
        
        state = DRIVE_FORWARD;
    }
}


void readUltrasonicSensors()
{
    // this is for ultrasonic 2. You may need to change these commands if you use a different sensor.
    digitalWrite(ultrasonic2TrigPin, HIGH);
    delayMicroseconds(10);                  // keeps the trig pin high for at least 10 microseconds
    digitalWrite(ultrasonic2TrigPin, LOW);
    
    ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);
    ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}

// the following is for debugging errors in the console.
void debugOutput()
{
    if((millis() - timeSerialDelay) > serialPeriod)
    {
        Serial.print("ultrasonic2Distance: ");
        Serial.print(ultrasonic2Distance);
        Serial.print("cm");
        Serial.println();
        
        timeSerialDelay = millis();
    }
}