O vídeo do robô com um sensor de ultrasom que se desvia de obstáculos.
O código foi baseado em um projeto no site arduino.cc. Este robô é uma versão melhorada do veículo autônomo mostrado na parte 6 do tutorial de Arduino.
O código-fonte abaixo.
#include <NewPing.h>
#include <Servo.h>
#define trigPin 9
#define echoPin 10
Servo servo;
int posservo;
int IN1=4;
int IN2=5;
int IN3=7;
int IN4=2;
int ENA=3;
int ENB=6;
#define max_distance 200
boolean goesForward = false;
int distance;
NewPing sensor(trigPin,echoPin,max_distance);
void setup() {
servo.attach(8);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(echoPin,INPUT);
pinMode(trigPin,OUTPUT);
Serial.begin(9600);
servo.write(90);
delay(150);
}
void loop(){
analogWrite(ENA,210);
analogWrite(ENB,190);
int distanceR=0;
int distanceL=0;
distance = readPing();
Serial.println(distance);
if(distance <= 20){//Verifica se há obstáculo à frente.
moveStop();
delay(300);
moveBackward();
delay(400);
moveStop();
delay(300);
distanceR = lookRight();
delay(300);
distanceL = lookLeft();
delay(300);
if (distanceR >= distanceL){
turnRight();
moveStop();
}
else{
turnLeft();
moveStop();
}
}else{
moveForward();
}
}
int lookRight(){
servo.write(0);
delay(150);
int distance = readPing();
delay(100);
servo.write(90);
return distance;
}
int lookLeft(){
servo.write(180);
delay(150);
int distance = readPing();
delay(100);
servo.write(90);
return distance;
delay(100);
}
int readPing(){
delay(70);
int cm = sensor.ping_cm();
if (cm==0){
cm=250;
}
return cm;
}
void moveStop(){
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
void turnRight(){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
delay(600); //Valor deve mudar dependendo da carga da bateria.
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}
void turnLeft(){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
delay(600); //Valor deve mudar dependendo da carga da bateria.
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}
void moveForward(){
if(!goesForward){
goesForward=true;
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}
}
void moveBackward(){
goesForward=false;
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
