Arduino Robot Code

arduino_robot

/* 
Original code by Nick Brenn
Modified by Marc de Vinck
Make Projects Arduino-based 4WD robot
http://makeprojects.com/Project/Build-your-own-Arduino-Controlled-Robot-/577/1 
 */
#include <AFMotor.h> 
//AF_DCMotor motor1(1, MOTOR12_8KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR12_1KHZ);
//AF_DCMotor motor4(4, MOTOR12_1KHZ);
const int pingPin = 19;
// ------------------------------------------------------------------------------
void setup()
{
Serial.begin(9600); // set up Serial library at 9600 bps
//motor1.setSpeed(200); // Sets the speed for all the motors 1 - 4 (255 is the maximum speed)
motor2.setSpeed(200);
motor3.setSpeed(200);
//motor4.setSpeed(200);
}
long ping()
{
long duration, inches;
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
long microsecondsToInches(long microseconds);
inches = microsecondsToInches(duration);
return inches;
} // ------------------------------------------------------------------------------
void forward() // This function moves all the wheels forward
{
//motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
//motor4.run(FORWARD);
}
// ------------------------------------------------------------------------------
void backward() // This function moves all the wheels backwards
{
//motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
//motor4.run(BACKWARD);
}
// ------------------------------------------------------------------------------
void haltMotors() // This function stops all the motors (It is better to stop the motors before changing direction)
{
//motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
//motor4.run(RELEASE);
}
// ------------------------------------------------------------------------------
void turnRight() // This function turns the robot right
{
//motor1.run(FORWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
//motor4.run(BACKWARD);
}
// ------------------------------------------------------------------------------
void loop() { // This is the main program that will run over and over
long duration, inches; // Declare the variables "duration" and "inches"duration = pulseIn(pingPin, HIGH);
inches = ping(); // Set the inches variable to the float returned by the ping() function.
//Serial.print(inches);
//Serial.print("in, ");
//Serial.println();
//while (inches >8){ // While the robot is 8 inches away from an object.
//inches = ping();
forward(); // Move the robot forward.
//}
haltMotors(); // Stop the motors for 2 seconds, before changing direction.
delay(1000);
while (inches < 10){ // Until the robot is 10 inches away from the object, go backward.
inches = ping();
backward(); // Move the robot backward.
}
turnRight(); // Once the robot is done moving backward, turn the robot right.
delay(1500); // Note! Change this value (greater or smaller) to adjust how much the robot turns right
haltMotors();
delay(1000);
}
long microsecondsToInches(long microseconds){
return microseconds /74 / 2;
}