Implementación de sensores y sincronizacion de componentes
En esta tercera entrega se realizara la implementación de un segundo sensor ultrasonico que nos permitirá captar objetos a una altura de 5 a 8 cm
En esta segunda entrega el objetivo sera la sincronizacion de componentes en si el principal captar obstáculos en una altura de 2 cm a + de 10 cm y su distancia para evadir los obstáculos es de 50 cm.
La conexión de este dispositivo se realiza de la siguiente manera y su codificación la dejaremos en la parte de abajo
Codificación y sincronizacion de componentes
#include
#include
#define MOTOR_1_FORWARD 5
#define MOTOR_1_BACK 6
#define MOTOR_2_FORWARD 3
#define MOTOR_2_BACK 4
#define TRIG 12
#define ECHO 11
#define TRIG2 8
#define ECHO2 9
#define OBSTRACLE_DISTANCE2 50.0
#define TURN_DELAY2 200
#define HEAD_SERVO 2
#define OBSTRACLE_DISTANCE 50.0
#define TURN_DELAY 200
#define LOG false
Ultrasonic ultrasonic(TRIG, ECHO);
Ultrasonic ultrasonic2(TRIG2, ECHO2);
Servo headServo;
int servoAngle = 90;
int angleStep = 30;
float distance = 0;
float distance2 = 0;
void setup()
{
pinMode(MOTOR_1_FORWARD, OUTPUT);
pinMode(MOTOR_1_BACK, OUTPUT);
pinMode(MOTOR_2_FORWARD, OUTPUT);
pinMode(MOTOR_2_BACK, OUTPUT);
stopMove();
headServo.attach(HEAD_SERVO);
if(LOG) Serial.begin( 9600 );
}
void loop()
{
updateHeadAngle();
checkDistance();
checkDistance2();
moove();
moove2();
delay(10);
}
void checkDistance()
{
distance = ultrasonic.Ranging(CM);
if(LOG) Serial.println(distance);
}
void checkDistance2()
{
distance2 = ultrasonic2.Ranging(CM);
if(LOG) Serial.println(distance2);
}
void moove()
{
if( distance > OBSTRACLE_DISTANCE )
{
if(LOG) Serial.println("FORWARD");
goForward();
delay(TURN_DELAY);
}
else
{
stopMove();
checkObstracle();
}
}
void moove2()
{
if( distance2 > OBSTRACLE_DISTANCE2 )
{
if(LOG) Serial.println("FORWARD");
goForward();
delay(TURN_DELAY);
}
else
{
stopMove();
checkObstracle2();
}
}
void checkObstracle()
{
int obsLeft = 0;
int obsRight = 0;
// Count the obstacles from left and right side
for(servoAngle = 0; servoAngle <= 180; servoAngle += 10)
{
headServo.write(servoAngle);
delay(10);
checkDistance();
if(distance < OBSTRACLE_DISTANCE && servoAngle < 90) obsLeft++;
else if(distance < OBSTRACLE_DISTANCE) obsRight++;
}
if(LOG) Serial.print("TURN");
if(obsLeft && obsRight)
{
goBack();
delay(TURN_DELAY * 2);
if(obsLeft < obsRight) goLeft();
else goRight();
delay(TURN_DELAY);
}
else if(obsRight)
{
goLeft();
delay(TURN_DELAY);
}
else if(obsLeft)
{
goRight();
delay(TURN_DELAY);
}
else
{
goForward();
delay(TURN_DELAY);
}
}
void checkObstracle2()
{
int obsLeft = 0;
int obsRight = 0;
checkDistance2();
if(distance2 < OBSTRACLE_DISTANCE2 ) obsLeft++;
else if(distance2 < OBSTRACLE_DISTANCE2) obsRight++;
if(LOG) Serial.print("TURN");
if(obsLeft && obsRight)
{
stopMove();
goBack();
delay(1000);
if(obsLeft < obsRight) goLeft();
else
stopMove();
goRight();
delay(1000);
}
else if(obsRight)
{
stopMove();
goLeft();
delay(1000);
}
else if(obsLeft)
{
stopMove();
goRight();
delay(1000);
}
else
{
stopMove();
goForward();
delay(1000);
}
}
void updateHeadAngle()
{
headServo.write(servoAngle);
servoAngle += angleStep;
if(servoAngle >= 150)
{
servoAngle = 150;
angleStep *= -1;
}
if(servoAngle <= 30)
{
servoAngle = 30;
angleStep *= -1;
}
}
void goForward()
{
digitalWrite(MOTOR_1_FORWARD, HIGH);
digitalWrite(MOTOR_1_BACK, LOW);
digitalWrite(MOTOR_2_FORWARD, HIGH);
digitalWrite(MOTOR_2_BACK, LOW);
}
void goBack()
{
digitalWrite(MOTOR_1_FORWARD, LOW);
digitalWrite(MOTOR_1_BACK, HIGH);
digitalWrite(MOTOR_2_FORWARD, LOW);
digitalWrite(MOTOR_2_BACK, HIGH);
}
void goLeft()
{
digitalWrite(MOTOR_1_FORWARD, LOW);
digitalWrite(MOTOR_1_BACK, HIGH);
digitalWrite(MOTOR_2_FORWARD, LOW);
digitalWrite(MOTOR_2_BACK, LOW);
}
void goRight()
{
digitalWrite(MOTOR_1_FORWARD, LOW);
digitalWrite(MOTOR_1_BACK, LOW);
digitalWrite(MOTOR_2_FORWARD, LOW);
digitalWrite(MOTOR_2_BACK, HIGH);
}
void stopMove()
{
digitalWrite(MOTOR_1_FORWARD, LOW);
digitalWrite(MOTOR_1_BACK, LOW);
digitalWrite(MOTOR_2_FORWARD, LOW);
digitalWrite(MOTOR_2_BACK, LOW);
}
