Hey guys,
I was wondering if you could offer advice.
I have built the avoidance robot.
I would now like to add to it.
I would like to add leds and
possibly a speaker, so when the
robot approaches a wall maybe the leds turn on. When it reverses maybe it beeps.
How would I do this.?
How do I incorperate it into the code.
By this I mean say for example
If ping distance is less then 5cm then: led on
type of thing?
My coding is practically non existant. I am still learning
and bit by bit understanding.
Thanks guys.
#define LeftMotorForward 8
#define LeftMotorBackward 9
#define RightMotorForward 10
#define RightMotorBackward 11
#define USTrigger 3
#define USEcho 2
#define MaxDistance 100
#define LED 13
//Here we have created two 'objects', one for the servo and one for the ultrasonic sensor
Servo servo;
NewPing sonar(USTrigger, USEcho, MaxDistance);
//Below we are creating unsigned integer variables which we will use later on in the code. They are unsigned as they will only have postive values
unsigned int duration;
unsigned int distance;
unsigned int FrontDistance;
unsigned int LeftDistance;
unsigned int RightDistance;
unsigned int Time;
unsigned int CollisionCounter;
void setup() //This block happens once on startup
{
Serial.begin(9600); //I have included the serial initialize but commented it out, if you want to debug and print information to the serial monitor just uncomment
//Here we are setting the pin modes. As we will sending out signals from the pins we set them as outputs
pinMode(LeftMotorForward, OUTPUT);
pinMode(LeftMotorBackward, OUTPUT);
pinMode(RightMotorForward, OUTPUT);
pinMode(RightMotorBackward, OUTPUT);
pinMode(LED, OUTPUT);
servo.attach(6); //The servo is attached to pin 4
}
void loop() //This block repeats itself while the Arduino is turned on
{
servo.write(90); //Rotate the servo to face the front
scan(); //Go to the scan function
FrontDistance = distance; //Set the variable FrontDistance to the value of the distance returned from the scan function
Serial.println("Front distance = ");
Serial.print(distance);
if(FrontDistance > 40 || FrontDistance == 0) //If there is nothing infront of the robot within 40cm or the distance value is 0 (which for the newping libary means no ping was returned) then...
{
moveForward(); //Go to the moveForward function
}
else //Else (if there is something infront of the robot within 40cm) then...
{
CollisionCounter = CollisionCounter + 1;
moveStop(); //Go to the moveStop function
navigate();
}
}
void moveForward() //This function tells the robot to go forward
{
Serial.println("");
Serial.println("Moving forward");
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorBackward, LOW);
digitalWrite(RightMotorForward, HIGH);
}
void moveBackward() //This function tells the robot to move backward
{
Serial.println("");
Serial.println("Moving backward");
digitalWrite(LeftMotorForward, LOW);
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorForward, LOW);
digitalWrite(RightMotorBackward, HIGH);
}
void moveLeft() //This function tells the robot to turn left
{
Serial.println("");
Serial.println("Moving left");
digitalWrite(LeftMotorForward, LOW);
digitalWrite(LeftMotorBackward, HIGH);
digitalWrite(RightMotorBackward, LOW);
digitalWrite(RightMotorForward, HIGH);
}
void moveRight() //This function tells the robot to turn right
{
Serial.println("");
Serial.println("Moving right");
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(LeftMotorForward, HIGH);
digitalWrite(RightMotorForward, LOW);
digitalWrite(RightMotorBackward, HIGH);
}
void moveStop() //This function tells the robot to stop moving
{
Serial.println("");
Serial.println("Stopping");
digitalWrite(LeftMotorBackward, LOW);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
}
void scan() //This function determines the distance things are away from the ultrasonic sensor
{
Serial.println("");
Serial.println("Scanning");
Time = sonar.ping();
distance = Time / US_ROUNDTRIP_CM;
delay(500);
}
void navigate()
{
Serial.println("There's an obstacle!");
servo.write(167); //Move the servo to the left (my little servos didn't like going to 180 so I played around with the value until it worked nicely)
delay(1000); //Wait half a second for the servo to get there
scan(); //Go to the scan function
LeftDistance = distance; //Set the variable LeftDistance to the distance on the left
Serial.println("Left distance = ");
Serial.print(distance);
servo.write(0); //Move the servo to the right
delay(1000); //Wait half a second for the servo to get there
scan(); //Go to the scan function
RightDistance = distance; //Set the variable RightDistance to the distance on the right
Serial.println("Right distance = ");
Serial.print(distance);
if(abs(RightDistance - LeftDistance) < 5)
{
moveBackward(); //Go to the moveBackward function
delay(200); //Pause the program for 200 milliseconds to let the robot reverse
moveRight(); //Go to the moveRight function
delay(100); //Pause the program for 200 milliseconds to let the robot turn right
}
else if(RightDistance < LeftDistance) //If the distance on the right is less than that on the left then...
{
moveLeft(); //Go to the moveLeft function
delay(100); //Pause the program for half a second to let the robot turn
}
else if(LeftDistance < RightDistance) //Else if the distance on the left is less than that on the right then...
{
moveRight(); //Go to the moveRight function
delay(100); //Pause the program for half a second to let the robot turn
}
}
