Cómo es de esperar, podemos incorporar la corrección de velocidad a efectos de igualar velocidades cuando el robot avanza sin obstáculos al frente, y de paso, aplicar algunas reglas de "buen programador"... o mi mejor intento en dicho sentido.
La nueva versión del programa completo:
#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, D;
int Vel_0, Vel_1;
byte aux;
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());
Pulsos_0 = 0;
Pulsos_1 = 0;
msTranscurridos = 0;
Vel_0 = 0;
Vel_1 = 0;
D = 0;
aux = 0;
Sentido(DetineHIGH);
delay(1500);
ActivarInterrupciones();
}
void loop() {
D = Distancia();
Vel_0 = V_Crucero;
Vel_1 = V_Crucero;
if (aux > 4) {
(D < 6 && D > 0)? Esquivar(): Avanzar();
} else aux++;
}
unsigned long Distancia() {
digitalWrite(DISPARO, LOW);
delayMicroseconds(2);
digitalWrite(DISPARO, HIGH);
delayMicroseconds(10);
digitalWrite(DISPARO, LOW);
return int(0.017 * pulseIn(ECO, HIGH));
}
void Avanzar() {
Sentido(Avanza);
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;
ActivarInterrupciones();
}
}
void Esquivar() {
DetenerInterrupciones();
Sentido(DetineLOW);
delay(50);
Sentido(Retrocede);
ContarPulsos(20);
Sentido(DetineLOW);
delay(50);
Sentido(random(Giro_1, DetineLOW)); //Gira
ContarPulsos(random(10, 25));
Sentido(DetineLOW);
delay(50);
ActivarInterrupciones();
}
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);
}
Es el objetivo del buen programador que su código sea tan claro que se interprete fácilmente por cualquier programador... pero cómo no es mi caso, trataré de explicarlo lo mejor que pueda.
En primer lugar, se declaran las constantes, algunas usando la directiva de Pre-procesador #define para definir valores constantes, otras usando la palabra reservada const, también para el mismo objetivo, en ambos casos solo defino constantes, la finalidad de los valores constantes es la de proporcionar una mayor legibilidad al código fuente y ayudar a que el mantenimiento del programa sea más sencillo y cómodo. Las constantes mejoran la legibilidad del código al dar un nombre a un valor.
El criterio que usé: las constantes que se utilizan en el programa las declaré con #define, las constantes que hacen referencia a los pines del metaboar, con const.
Se puede leer un poco más sobre las distintas formas de declarar constantes en:
Con respecto a la declaración de variables globales y las dos funciones asociadas a las interrupciones, no hay nada que decir, salvo que renombré algunas variables con nombres un poquito más significativos según sus usos en el programa.
En lo que respecta al la función setup(), es muy clara por si sola, su función es la de configurar los pines según su uso, así como los valores iniciales de las variables globales, y algunas acciones relacionadas con la inicialización del hardware propiamente dicho.
En loop(), el único cambio es que las variables que hacen al PWM correspondiente a cada motor, pueden verse alteradas, modificadas según los pulsos contados en las interrupciones, por ende, se fijan a un valor predeterminado en cada ciclo de ejecución de esta función.
La función Distancia() no tiene modificación, nada que comentar, solo recordar que debe medir la distancia al objeto que, eventualmente, se encuentre dentro del rango de acción del sensor.
En la función Avanzar() si hay cambios importantes:
/*Configurar los pines de los motores que hacen a la dirección
que se corresponda con lo que consideramos como "adelante"*/
Sentido(Avanza);
/*se actualiza la variable msTrancurridos, para referencia de
la siguiente detención de interrupciones*/
msTranscurridos = millis();
/*si los pulsos leídos de un motor son mayores que los del
otro motor, entonces se reduce la variable que hace a la
velocidad del motor que gira más rápido, proporcionalmente
a la diferencia existente entre ambos motores*/
/*a efectos de evitar reducir en demasía la velocidad de un
motor con respecto al otro, se mantiene la diferencia
dentro de cierto límite*/
if (Vel_0 < (Vel_1 * LimCorrecn)) Vel_0 = Vel_1 * LimCorrecn;
/*puesta a cero de las variables que cuentan los pulsos de
ambos motores, para comenzar un nuevo conteo mientras no se
cumpla otro ciclo de detención de interrupciones*/
Pulsos_0 = 0;
Es aquí donde se realiza la corrección de las velocidades de los motores a efectos de lograr una dirección un poco más recta cuando así debería ser.
En cuanto a la función Esquivar(), es prácticamente igual a la usada en la entrada Evitando Obstáculos (3), salvo el echo de que se detienen las interrupciones durante el proceso para esquivar un obstáculo.
En la función Sentido() agregué algunas posibles direcciones más, así cómo la actualización de los pulsos PWM en los pines correspondientes, pero en esencia, sigue siendo lo mismo de otras entradas.
Por último, las funciones DetenerInterrupciones() y ActivarInterrupciones() solo hacen a las llamadas a las instrucciones que realizan las acciones correspondientes.
Y eso es todo para esta entrada.
--------------------------------------------------------------------------------------------------------------------------La función Distancia() no tiene modificación, nada que comentar, solo recordar que debe medir la distancia al objeto que, eventualmente, se encuentre dentro del rango de acción del sensor.
En la función Avanzar() si hay cambios importantes:
void Avanzar() {
/*Configurar los pines de los motores que hacen a la dirección
que se corresponda con lo que consideramos como "adelante"*/
Sentido(Avanza);
/*Cada vez que se alcance un tiempo determinado...*/
if (millis() - msTranscurridos >= T_Interrup) {
/*se detienen las interrupciones, se deja de contar los pulsos
de las ruedas*/
if (millis() - msTranscurridos >= T_Interrup) {
/*se detienen las interrupciones, se deja de contar los pulsos
de las ruedas*/
DetenerInterrupciones();
/*se actualiza la variable msTrancurridos, para referencia de
la siguiente detención de interrupciones*/
msTranscurridos = millis();
/*si los pulsos leídos de un motor son mayores que los del
otro motor, entonces se reduce la variable que hace a la
velocidad del motor que gira más rápido, proporcionalmente
a la diferencia existente entre ambos motores*/
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));
/*a efectos de evitar reducir en demasía la velocidad de un
motor con respecto al otro, se mantiene la diferencia
dentro de cierto límite*/
if (Vel_0 < (Vel_1 * LimCorrecn)) Vel_0 = Vel_1 * LimCorrecn;
if (Vel_1 < (Vel_0 * LimCorrecn)) Vel_1 = Vel_0 * LimCorrecn;
/*puesta a cero de las variables que cuentan los pulsos de
ambos motores, para comenzar un nuevo conteo mientras no se
cumpla otro ciclo de detención de interrupciones*/
Pulsos_0 = 0;
Pulsos_1 = 0;
/*se activan las interrupciones, se inicia el conteo de los
pulsos leídos desde las ruedas con los encoders*/
ActivarInterrupciones();
/*se activan las interrupciones, se inicia el conteo de los
pulsos leídos desde las ruedas con los encoders*/
ActivarInterrupciones();
} //fin del condicional de medición de tiempo
} //fin de la función Avanzar
Es aquí donde se realiza la corrección de las velocidades de los motores a efectos de lograr una dirección un poco más recta cuando así debería ser.
En cuanto a la función Esquivar(), es prácticamente igual a la usada en la entrada Evitando Obstáculos (3), salvo el echo de que se detienen las interrupciones durante el proceso para esquivar un obstáculo.
En la función Sentido() agregué algunas posibles direcciones más, así cómo la actualización de los pulsos PWM en los pines correspondientes, pero en esencia, sigue siendo lo mismo de otras entradas.
Por último, las funciones DetenerInterrupciones() y ActivarInterrupciones() solo hacen a las llamadas a las instrucciones que realizan las acciones correspondientes.
Y eso es todo para esta entrada.
No hay comentarios.:
Publicar un comentario