Driver 4 Servomotores por i2c Remade

Saludos,

Edito: 21/6/2020

Pues no ha llovido ni nada. A las buenas, vengo a citarme yo mismo, el anterior post es incorrecto de ahí de añadirle el FAIL en grandote el motivo es que no funciona. Para empezar el caso de no funcione es que las salidas del PWM no van a 50 hz que es la premisa para que funcione un servo. Por ello y sin borrar el anterior porque creo que de los errores se aprende más de lo que uno cree. Voy a rehacer el post.

Continuando con mis prototipos, hoy os presento una placa para controlar 4 servomotores, vendría a substituir la placa que os mostré en la entrada anterior , pero con otro tipo de motores. Empleamos el PIC16F1509 el modelo superior del PIC16F1503, más memoria más pines más tamaño. y la oportunidad de emplear los 4 PWM para obtener un control de los servomotores.

Figura 1 placa del prototipo

Los servomotores funcionan a una frecuencia de 50 Hz , eso equivale a un ciclo de reloj de 20 ms, y el régimen de trabajo se desarrolla entre 1 y  2 ms. Este dato puede variar en función de los Servomotores que empleemos.  La placa dispone de un conector de alimentación extra para los servos, para los test y las pruebas he empleado la tensión propia del i2c y los servomotores DFR0399 un micrometal de 75:1 con control de servo. En este caso en vez de controlar la posición, controlamos la velocidad y la dirección.

Conexiones

Pic	Función
RC5	Servo01 à pwm1 en el pic
RC3	Servo02 àpwm2 en el pic
RB7	LED
RA0	I/O o ANALOGICA
RA1	I/O o ANALOGICA
RA2	Servo03 à pwm3 en el pic
RC1	Servo04 à pwm4 en el pic
RB4	SDA
RB6	SCL

Control de los ServoMotores

Al emplear el modelo PICF1509 podemos usar los cuatro PWM del pic para que nos generen un pulso con una frecuencia de 50Hz, o lo que es lo mismo 20 ms. Que el tiempo que necesitan la mayoría de los servos para funcionar.

Esto lo conseguimos:

• Para conseguir este intervalo de tiempo en el Timer 2, es necesario poner el reloj interno del pic a 8Mhz en vez de los 16Mhz que permite.

setup_timer_2(T2_DIV_BY_16,250,10);      //2,0 ms overflow, 20,0 ms interrupt

Nota: Es incorrecto la explicación, para empezar vamos a explicar que es cada parte :

T2_DIV_BY_16 es el Preescaler en este caso 16 y puede ser 16,4,1.

PR2 o carga del  Timer2 es el segundo valor en este caso 250.

Postscale es el tercer valor en nuestro caso 10.

Si fuéramos a emplear una interrupción en el Timer2 se daría cada 20ms y tendría una frecuencia de 50HZ, pero para el PWM debemos fijarnos en el dato del overflow que nos indica que  es cada 2,0 ms eso es un frecuencia de 500H, y eso es lo que no va a dar las salidas de PWM.  Como conseguimos los 50HZ, luego lo explico.

• .Vamos a hacer algunos números.

Nota: volvemos al mismo error anterior la frecuencia de salida es 500Hz no 50Hz, otra forma de detectar el error es que la resolución máxima que dispone el pic es de 10 bits es decir 1024.

Estos valores se adecuan mas a los 10 bits de resolución, pero nos dejan un margen de maniobra muy reducido 90 valores para determinar 180 grados. Dependiendo de la aplicación podría funcionarnos.

Pero la cuestión es que no es fácil que nos funcione, vamos a resolver el primer problema conseguir 50hz en las salidas. Para conseguirlo tenemos que poner el oscilador interno a funcionar a 500KHZ.

setup_timer_2(T2_DIV_BY_16,155,1);      //20,0 ms overflow, 20,0 ms interrupt

Y pensamos Bueno toda ira más lento, pero si funciona…

La comunicación I2C no va a esa velocidad a no ser que la bajes con lo que perjudicas al resto de sistemas. 

Controlar servos con PWM es una mala alternativa, el control se simplifica, pero te quedas sin proceso.

Entonces como controlamos 4 servos con un pic mediante I2C

Este es un tema que llevo mucho tiempo plantándome y que no hallaba fácil solución, utilizando una interrupción en timer2 a 20ms.

setup_timer_2(T2_DIV_BY_16,250,10);      //2,0 ms overflow, 20,0 ms interrupt

1 servo, pan comido	#INT_TIMER2 void  TIMER2_isr(void) {   output_high(PIN_C3);   delay_us(pulse_time);   output_low(PIN_C3); }
2 servos, no hay problema, miramos quien tiene el pulso mas corto.	#INT_TIMER2 void  TIMER2_isr(void){    output_high(PIN_C3);    output_high(PIN_C5);    if(pr1>pr2){       delay_us(pr2);       output_low(PIN_C5);       delay_us(pr1-pr2);       output_low(PIN_C3);    }else if (pr1<pr2){       delay_us(pr1);       output_low(PIN_C3);       delay_us(pr2-pr1);       output_low(PIN_C5);    }else{    delay_us(pr1);    output_low(PIN_C3);    output_low(PIN_C5);    } }
3 servos ya se complican un poco mediante este sistema, el tiempo de ordenar cual va antes puede hacerse mas alto que la misma interrupción. Pero se podría hacer…

Con cuatro ya lo tengo probado que no va, entonces como hacerlo. Pues de una manera absurdamente simple. Gracias por abrirme los ojos.

Si tengo 20 ms de interrupción y 4 servos, porque no reservo 5 ms de la interrupción para cada servo. A efectos practico son 4 señales de 50Hz desfasada 5 ms la una de la otra pero en cada ciclo. Si me apuras y marcas un tiempo de 2,5 valido para casi la mayoría de servos, se podría controlar 8 servos diferentes.El código:

#INT_TIMER2 void  TIMER2_isr(void) {    // cada 3 ms actua un servo empezamos por RC5    output_high(PIN_C5);    delay_us(servo[0]);    output_low(PIN_C5);    delay_us(3000-servo[0]);    /////////////////////////    output_high(PIN_C3);    delay_us(servo[1]);    output_low(PIN_C3);    delay_us(3000-servo[1]);    /////////////////////////    output_high(PIN_C1);    delay_us(servo[2]);    output_low(PIN_C1);    delay_us(3000-servo[2]);    //////////////////////////    output_high(PIN_A2);    delay_us(servo[3]);    output_low(PIN_A2);    }

Las duraciones del pulso se guardan en un vector int16_t servo[4]={1500,1500,1500,1500};

Se podría enviar la posición en un int16_t  y tener un control total y no un valor de conversión pero no todo el mundo lleva bien lo de trabajar con microsegundos.

Para el nuevo control los valores max=2200, min=800 y cero =1500.

 

Modo individual

El control se simplifica mucho. Convertimos en valor de ángulo en su equivalente para el ancho del pulso del pwm.

•         Posición del servomotor àservo[0..3], ángulo[0..180]
•         Posición del servomotor con delay predefinido àservo[0..3], ángulo[0..180]
•         Posición del servomotor con delay àservo[0..3], ángulo[0..180], delay(int16)

 Ejemplo de código  control de cada servo individual

int16_t servo[4];

void servo_individual(int nServo, int angle)

{

  servo[nServo]=angle;

}

Modo Coordinado

En este modo va en función de la aplicación que vamos a realizar. Si empleamos en ejemplo de los motores, el control de un vehículo con tracción a las cuatro ruedas. En este sentido el ángulo se interpreta como velocidad.

0°	90°	180°
Máxima velocidad ßß	Stop	Máxima velocidad àà

Dirección àForward, Backward, Lef, Right, Turn_left, Turn_right, Stop

·        Modo coordinado        

o   Instrucción àPalabra de control, ángulo (0..180)

§  0001 xxxx à Stop

§  0000 0011 à Forward

§  0000 1100 à Backward

§  0000 1010 à Left

§  0000 0101 à Right

§  0000 1111 à Turn_left

§  0000 0000 à Turn_right

o   Instrucción con delay predefinido à Palabra de control, ángulo (0..180)

o   Instrucción con delayà Palabra de control, ángulo (0..180),delay(int16)

Ejemplo de función en Modo coordinado

void servomotor_con_all( int control,int angle)

{

   if(!bit_test(control,4))  //non STOP

   {

      if(!bit_test(control,3)) servo[0]=angle;     

      else  servo[0]=max-angle;

      if(!bit_test(control,2)) servo[1]=angle;   

      else servo[1]=max-angle;

      if(!bit_test(control,1)) servo[2]=angle;    

      else servo[2]=max-angle;

      if(!bit_test(control,0)) servo[3]=angle;    

      else servo[3]=max-angle;

      

   }else   stop_all();  

}

El control mediante PWM simplifica mucho el tipo de control, tanto el individual como el coordinado.

Instrucciones I2c para controlar el driver

cmd	Datos	Función
0x01	--	Led on
0x02	--	Led off
0x10	ServoMotor, ángulo	control motor individual
0x11	ServoMotor,ángulo	control motor individual con delay por defecto
0x12	ServoMotor,ángulo,delayH,delayL	control motor individual con delay enviado
0x15	Control,ángulo	Control Servomotores
0x16	Control,ángulo	Control Servomotores con delay por defecto
0x17	Control,ángulo, delayH,delayL	Control Servomotores con delay enviado
0x20	delayH,delayL	Cambio delay
0x30	--	Leer la entrada analógica A0
0x31	--	Leer la entrada analógica A1
0x32	--	Leer variable de control 4660

Esta placa esta interesante pues ofrece poder controlar diferentes servos sin tener que saturar a la cpu, con varias placas de este tipo conectadas a un máster se podría controlar un hexapodo mediante mensajes de i2c. Mi obsesión por este protocolo es alta.


Anakleto.