Laboratorio 15

EVASOR DE OBSTÁCULOS Y SEGUIDOR DE LÍNEA

Coronel Chambi, Jose Luis

Espinal Ahuanlla, Hugo Neill

Programación y explicación en ARDUINO

CÓDIGO

#define lineTracking1 11
#define lineTracking2 4
#define lineTracking3 2

/*Sensor Ultrasónico*/
int Echo = A4;
int Trig = A5;
int in1 = 9;
int in2 = 8;
int in3 = 7;
int in4 = 6;
int ENA = 10;
int ENB = 5;
int ABS = 110;
int rightDistance = 0, leftDistance = 0, middleDistance = 0;

/*Sensores Infrarrojos*/
bool num1, num2, num3;
bool flag;

/*Motores*/
void _mForward()
{
  analogWrite(ENA, ABS);
  analogWrite(ENB, ABS);
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}

void _mBack()
{
  analogWrite(ENA, ABS);
  analogWrite(ENB, ABS);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}

void _mleft()
{
  analogWrite(ENA, ABS);
  analogWrite(ENB, ABS);
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}

void _mright()
{
  analogWrite(ENA, ABS);
  analogWrite(ENB, ABS);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}
void _mStop()
{
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
}

int Distance_test()
{
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);
  float Fdistance = pulseIn(Echo, HIGH);
  Fdistance = Fdistance / 58;
  return (int)Fdistance;
}

void setup() {

  pinMode (11, INPUT);
  pinMode (4, INPUT);
  pinMode (2, INPUT);

  Serial.begin(9600);

  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  _mStop();
}

void loop()
{
  num1 = digitalRead(lineTracking1);
  num2 = digitalRead(lineTracking2);
  num3 = digitalRead(lineTracking3);

  if (!num1 && num2 && !num3)
  {
    _mForward();
  }
  else if (num1 && num2 && num3)
  {
    _mForward();
  }
  else if (num1 && !num2 && !num3)
  {
    _mleft();
  }
  else if (num1 && num2 && !num3)
  {
    _mleft();
  }
  else if (!num1 && !num2 && num3)
  {
    _mright();
  }
  else if (!num1 && num2 && num3)
  {
    _mright();
  }
// distancia //
  middleDistance = Distance_test();

  if (middleDistance <= 10)
  {
    _mStop();
    delay(500);
    _mleft();
    delay(1000);
    _mForward();
    delay(1000);
    _mright();
    delay(1300);
    _mForward();
    delay(1050);
  }
}

VÍDEO DEL FUNCIONAMIENTO