Función MOTORES SL

☼ Las pruebas fueron realizadas en el Robot MECHA 22, los resultados fueron:

El encabezado del código:

// ► ETIQUETAS FUNCION MOTOR //

#define pwmi 3     // velocidad motor izq

#define mot_izq_A1 5

#define mot_izq_A2 4

#define pwmd 9     // velocidad motor der

#define mot_der_B1 7

#define mot_der_B2 6

void setup() {

// ► SET DE PINES LIBRERÍA MOTOR //  

  pinMode(mot_izq_A1,OUTPUT);

  pinMode(mot_izq_A2,OUTPUT);

  pinMode(mot_der_B1,OUTPUT);

  pinMode(mot_der_B1,OUTPUT);

}

☼ Resultados de la prueba de sentido de giro de los motores.

Con estos datos podemos escribir el código de la función para controlar la velocidad y el sentido de giro de los motores.

La velocidad de los mismos estará calculado por el control PID.

☼ Observación: los cálculos de velocidad del control PID pueden darnos valores positivos o negativos.

• Positivos - Hacia adelante.

• Negativos - Hacia atrás.

▬ Tenemos que programar nuestra función de manera que esta pueda controlar el sentido de giro de los motores aun con valores negativos, ya que el PWM del micro solo toma valores positivos.

☼ Función para control de motores:

/*creamos la función motores con dos variables tipo entero VmIzq - VmDer cunado llamemos a la función, podemos darle valor a estas variables*/

void motores (int VmIzq, int VmDer) {     

/*Dirección Motor Izquierdo*/

  if (VmIzq >= 0) {                        //si la velocidad calculada es mayor o igual a 0, el giro es adelante

    digitalWrite(mot_izq_A1,HIGH);         //colocamos los estados para sentido de giro adelante en el motor izq.

    digitalWrite(mot_izq_A2,LOW);

  }

  else {                                   // si la velocidad no es mayor a 0, esta será negativa, giro atrás

    digitalWrite(mot_izq_A1,LOW);          // colocamos los estados para sentido de giro atrás en el motor izq.

    digitalWrite(mot_izq_A2,HIGH);

    VmIzq = VmIzq * -1;                    // la velocidad no puede ser negativa, así que le cambiamos el signo

  }                                        // por que el sentido de giro atrás ya fue establecido.

/*Dirección Motor Izquierdo*/

    if (VmDer >= 0) {                      // si la velocidad calculada es mayor o igual a 0, el giro es adelante

    digitalWrite(mot_der_B1,HIGH);         // colocamos los estados para sentido de giro adelante en el motor der.

    digitalWrite(mot_der_B2,LOW);

  }

  else {                                   // si la velocidad no es mayor a 0, esta será negativa, giro atrás

    digitalWrite(mot_der_B1,LOW);          // colocamos los estados para sentido de giro atrás en el motor der.

    digitalWrite(mot_der_B2,HIGH);

    VmDer = VmDer * -1;                    // la velocidad no puede ser negativa, así que le cambiamos el signo

  }                                        // por que el sentido de giro atrás ya fue establecido.

  analogWrite(pwmi,VmIzq);                 // escribimos la velocidad en la salida PWM del micro para motor izq.

  analogWrite(pwmd,VmDer);                 // escribimos la velocidad en la salida PWM del micro para motor der.

}

/* Con el nuevo bucle del código los valores y sentidos se ajustaran a lo que fue calculado por el PID*/

► Función de motores en el bucle de marcha.

Para llamar a la función y cargarle los valores tenemos que colocar la siguiente linea de código:

        motores(0,0);     // en este caso la velocidad será 0 en cada motor.

Ejemplos de la función motores:

       motores(120,0);     // motor izq hacia adelante a 120 de velocidad - motor der en 0

       motores(0,120);     // motor izq en 0 - motor der hacia adelante a 120 de velocidad

       motores(-120,0);    // motor izq hacia atrás a 120 de velocidad - motor der en 0

       motores(0,-120);    // motor izq en 0 - motor der hacia atras a 120 de velocidad

☼ Comprobación de la función en el Robot MECHA 22.