I added the ping sensor into this demo, and also got tired of typing motordriver.goForward and ON an ON an ON.

So I turned the commands in modules, now I just need to finish the other direction modules and then add the second HC-SR04 (Which will require another head), and decide how to use the go backward command:  maybe if both sensor show as being in a corner (backward, left, forward)?

--------------------------------------------------------------------------------------------

// Demo function:The application method to drive the DC motor.

// Author:Frankie.Chu

// Date:20 November, 2012

#include "MotorDriver.h" //SeeedStudio Motor Library

#include "NewPing.h" //library for the SR04 ultrasonic sensor

#include "Servo.h" //Standard Servo Library

Servo HeadServo;

#define TRIGGER_PIN 2 //using analog pins as digital for SR04 sensor

#define ECHO_PIN 3 //because I just happened to plug them in here

#define MAX_DISTANCE 200 //the sensor can't measure much further then this distance

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))

int uS; //value of SR04 ultrasonic sensor

int Distance; //distance in cm of ultrasonic sensor

int Too_Close = 50;

//int pos = 90; //Center the Servo

int robotDirection = 0; //0 = forward, 1 = backward, 2 = left, 3 = right

//int servoDirection = 0; //Sweep Left or Right

int lastRobotDirection; //last direction of the robot

int distanceCenter;

//int distanceLeft;

//int distanceRight;

//int servoDelay = 2; //Sweep speed

void setup()

{

/*Configure the motor A to control the wheel at the left side.*/

/*Configure the motor B to control the wheel at the right side.*/

motordriver.init();

motordriver.setSpeed(75,MOTORB);

motordriver.setSpeed(90,MOTORA);

Serial.begin(9600);

}

void loop()

{

// sweepServo();

getDistance();

if (Distance < Too_Close)

{

Forward();

//delay(2000);

}

else if (Distance > Too_Close)

{

Left();

}

else

{

Stop();

}

}

void Forward()

{

// sweepServo();

motordriver.goForward();

Serial.print("Going Forward");

}

void Backward()

{

// sweepServo();

motordriver.goBackward();

}

void Left()

{

// sweepServo();

motordriver.goLeft();

Serial.print("Going Left");

}

void Right()

{

// sweepServo();

motordriver.goRight();

}

void Stop()

{

// sweepServo();

motordriver.stop();

}

/* Servo And Sensor Commands - BEGIN */

void getDistance()

{

runEvery(40) //loop for ultrasonic measurement

{

uS = sonar.ping();

Distance = uS / US_ROUNDTRIP_CM;

if (uS == NO_ECHO) // if the sensor did not get a ping

{

Distance = MAX_DISTANCE; //so the distance must be bigger then the max vaulue of the sensor

}

Serial.print("Ping Distance: "); //to check distance on the serial monitor

Serial.print(Distance);

Serial.println("cm");

}

}

/*void sweepServo()

{

runEvery(servoDelay) //this loop determines the servo position

{

if(pos < 165 && servoDirection == 0) // 165 = servo to the left

{

pos = pos + 5; // +1 was to slow

}

if(pos > 15 && servoDirection == 1) // 15 = servo to the right

{

pos = pos - 5;

}

}

if (pos == 165 )

{

servoDirection = 1; //changes the direction

}

if (pos == 15 )

{

servoDirection = 0; //changes the direction

}

HeadServo.write(pos); //move that servo!

}*/

/* Servo And Sensor Commands - END */