Tercera Entrega

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);

}