Buscando Obstáculos (2)

Se trata de hacer exactamente lo mismo que en la entrada anterior, pero evitando la librería "NewPing.h".

El programa es prácticamente el mismo, simplemente en lugar de usar el comando "Radar.ping_cm()" uso mi función para medir distancias, ya explicada, solo la renombré a "Distancia_cm()" que, creo, es un nombre un poquito más significativo.

Otra modificación (no necesaria en realidad) se encuentra dentro de la función "SonarAlarma()", donde en lugar de llamar a la función "Inicializar()" utilizo el comando "asm("jmp 0x0000")".

Este comando lo que hace es -desde asembler- saltar incondicionalmente a la primera línea del programa, es, por definirlo de algún modo, una especie de reset al programa en si, no al hardware. 

No existe ningún comando o directiva (al menos que yo conozca) que permita realizar un reset del ATMEGA, y, si bien podemos imaginarnos alguna simple electrónica para disparar el pin de reset del hardware, en la práctica no le encuentro necesidad, por lo menos en lo aquí descrito, tal vez algún día si.

A continuación el programa completo:

#define MaxDisObjt 150

#define MinDisObjt 10

#define Interrupn0 0

#define Interrupn1 1

#define LimCorrecn 0.75

#define T_Interrup 500

#define V_Crucero  200

#define Retrocede  0

#define Avanza     1

#define Giro_1     2

#define Giro_2     3

#define Giro_3     4

#define Giro_4     5

#define DetineLOW  6

#define DetineHIGH 7

const int Ecd_0   =  2;

const int Ecd_1   =  3;

const int DISPARO =  4;

const int ECO     =  5;

const int PWM_I   =  6;

const int MtrI1   =  7;

const int MtrI2   =  8;

const int PWM_D   = 11;

const int MtrD1   = 12;

const int MtrD2   = 13;

volatile byte Pulsos_0, Pulsos_1;

unsigned long msTranscurridos;

int Vel_0, Vel_1;

byte aux, busquedas, girar;

void ContarPulsos0() { Pulsos_0++; }

void ContarPulsos1() { Pulsos_1++; }

void setup() {

  pinMode(Ecd_0,   INPUT );

  pinMode(Ecd_1,   INPUT );

  pinMode(DISPARO, OUTPUT); 

  pinMode(ECO,     INPUT ); 

  pinMode(PWM_I,   OUTPUT);

  pinMode(MtrI1,   OUTPUT);

  pinMode(MtrI2,   OUTPUT);

  pinMode(PWM_D,   OUTPUT);

  pinMode(MtrD1,   OUTPUT);

  pinMode(MtrD2,   OUTPUT);

  randomSeed(millis());

  Inicializar();

}

void Inicializar() {

  Sentido(DetineLOW);

  Pulsos_0        = 0;

  Pulsos_1        = 0;

  msTranscurridos = 0;

  Vel_0           = 0;

  Vel_1           = 0;

  aux             = 0;

  busquedas       = 0;

  girar = random(Giro_1, DetineLOW);

  delay(5000);

}

void loop() {

  if (aux > 4)

    BuscarObjetivo();

  else {

    Distancia_cm();

    aux++;

  }

}

void BuscarObjetivo() {

  Vel_0 = V_Crucero * LimCorrecn;

  Vel_1 = V_Crucero * LimCorrecn;

  

  if (Distancia_cm()!=0 && Distancia_cm()<MaxDisObjt) Avanzar();

  else

    if (busquedas < 14) {

      Sentido(girar);

      ContarPulsos(3);

      Sentido(DetineLOW);

      busquedas++;

      delay(250);

    } else busquedas = 0;

}

void Avanzar() { 

  if (Distancia_cm() < MinDisObjt) SonarAlarma();

  else {

    ActivarInterrupciones();

    Vel_0 = V_Crucero;

    Vel_1 = V_Crucero;

    if (millis() - msTranscurridos >= T_Interrup) {

      DetenerInterrupciones();

      msTranscurridos = millis();

      if(Pulsos_0>Pulsos_1) Vel_0 = Vel_0 - ((Pulsos_0 - Pulsos_1));

      if(Pulsos_1>Pulsos_0) Vel_1 = Vel_1 - ((Pulsos_1 - Pulsos_0));

      if (Vel_0 < (Vel_1 * LimCorrecn)) Vel_0 = Vel_1 * LimCorrecn;

      if (Vel_1 < (Vel_0 * LimCorrecn)) Vel_1 = Vel_0 * LimCorrecn;

      Pulsos_0 = 0;

      Pulsos_1 = 0;

    } 

    Sentido(Avanza);

  }

} 

void Sentido(byte d) { 

  switch (d) {

    case Retrocede: 

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 

            break;

    case Avanza:

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, HIGH); 

            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, LOW);  

            break;

    case Giro_1: //ambos motores, uno al revés que el otro

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, LOW); 

            break;

    case Giro_2: //ambos motores, al revés que Giro 1

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, HIGH); 

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 

            break;

    case Giro_3: //un motor retrocede, el otro detenido

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, LOW); 

            break;

    case Giro_4: //al revés que Giro 3

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 

            break;

    case DetineLOW: 

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, LOW);  

            break;

    case DetineHIGH:

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, HIGH); 

            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, HIGH); 

            break;

  }

  analogWrite(PWM_I, Vel_0); 

  analogWrite(PWM_D, Vel_1); 

}

void ContarPulsos(byte p) {

  byte Pulsos_0    = 0;

  byte Pulsos_1    = 0;

  byte E_Encdr0    = LOW;

  byte E_Encdr1    = LOW;

  byte E_Encdr0Ant = LOW;

  byte E_Encdr1Ant = LOW;

  

  while (Pulsos_0 < p && Pulsos_1 < p) {

    E_Encdr0 = digitalRead(Ecd_0);

    E_Encdr1 = digitalRead(Ecd_1);

    if (E_Encdr0 != E_Encdr0Ant && E_Encdr0 == HIGH) Pulsos_0++;

    if (E_Encdr1 != E_Encdr1Ant && E_Encdr1 == HIGH) Pulsos_1++;

    E_Encdr0Ant = E_Encdr0;

    E_Encdr1Ant = E_Encdr1;

  }  

}

void DetenerInterrupciones() {

  detachInterrupt(Interrupn0);

  detachInterrupt(Interrupn1);

}

void ActivarInterrupciones() {

  attachInterrupt(Interrupn0, ContarPulsos0, CHANGE);

  attachInterrupt(Interrupn1, ContarPulsos1, CHANGE);

}

void SonarAlarma() {

  for(int i=0; i<5; i++) {

    Sentido(DetineHIGH);

    delay(500);

    Sentido(DetineLOW);

    delay(250);

  }

  asm("jmp 0x0000"); //Inicializar(); 

}

unsigned long Distancia_cm() {

  digitalWrite(DISPARO, LOW);

  delayMicroseconds(2);

  digitalWrite(DISPARO, HIGH);

  delayMicroseconds(10);

  digitalWrite(DISPARO, LOW);

  return int(0.017 * pulseIn(ECO, HIGH)); 

}

--------------------------------------------------------------------------------------------------------------------------