Control de servos Robot hexápodo (woody)


#1

Para ir abriendo boca os dejo por aquí el diseño de la nueva placa controladora de servos para el robot. Serán 3 como esta conectadas por I2C.

Está todavía sin acabar (falta una conexión por RS-232 para poner el dongle bluetooth y para programación) así como una revisión a fondo y algunos footprints de la PCB.

Es capaz de leer la posición de los servos a través de un 4º hilo (la salida del potenciómetro), así como la corriente consumida por cada uno de ellos.


#2

Aquí está la última revisión, enviada ya a fabricar.

Los esquemas son los siguientes:

En un par de semanas como mucho espero tener la PCB aquí en casa y empezaré a probar. Ya os contaré qué tal funciona el diseño.

:slight_smile:


#3

Que buena pinta tiene!!! si señor!
Eso estamos deseandolo ver ya funcionando! :cool:


#4

Ya está casi en la puerta de mi casa… esta tarde la monto (con suerte), y os cuento qué tal va.


#5

Haciendo los primeros experimentos con el control de consumo de los servos he visto que los servos consumen de forma pulsada, con pulsos de hasta 20ms (igual que los pulsos de control a la entrada).

Más anchos cuanto más fuerza ejercen.

Aquí os mando unas gráficas.

están obtenidas con maple así:

> fd:=fopen("d:\proyectos\robotica\hexapod\circuitos\servo_board\lectura_ztx_para_filtrar.txt",READ); > d:=readdata(fd,2);
>
[LEFT] > fclose(fd); [/LEFT]
> [B]plot(d,x=6000…6080); (aquí cambiando la zona que queremos ver, en ms)

[/B]El problema que genera a la hora de leer la corriente consumida con el servo es que no es suficiente con hacer una lectura.

Por ahora lo que he probado es a hacer 40 lecturas a lo largo del ciclo de 20ms y sumarlas (una integral discreta a lo largo de los 20ms) y dividir entre el número de muestras tomadas. Y parece funcionar bastante bien.

Eso sí, para 8 servos y 16 canales analógicos por leer salen 40 * 50 *16, es decir: 32000 muestras por segundo ':), o una muestra cada 31.25 uS … sin DMA creo que la cpu se me queda bastante cargadita. (en ese periodo la cpu de 60MIPS ejecuta 1875 instrucciones), no se cuantas gasto en la lectura.


#6

Estoy haciendo un hexapodo, parecido al tuyo, pero tengo un problema
con el programa.
Utilizo el pic 16f877 y como controladora para cada pata un 16f628;
de momento tengo implementado (comco prueba) en le programa el
movimiento de los servos usando la funcion delay_us() en C, pero a la
hora de realizar el programa con interrupciones temporizadas para
controlar el movimiento de los servos.Y no sé porqué me atasco aquí.
¿puedes explicarme como has hecho el progrma de control para una una pata?
De antemano gracias.
:-):slight_smile:

P.D.: Adjunto mi diseño del segundo eje para el servo, tal vez le sirva a alguien.


#7

Hola mandoa, te cuento, perdona el restraso

Yo para gestionar los servos utilizo un 16F689, que me da para generar 8 señales PWM “simultaneas”.

como verás en el código, cada 16F682 está controlado por I2C, y solo acepta comunicación I2C durante un breve periodo de tiempo (en el que emite una señal al maestro PWM_RTS y se gestiona en cycle0_handle_comms() ).

La señal no se genera para los 8 a la vez, sino que va alternándose de 2 en 2, con un bucle en ensamblador para cada uno, y sincronizados al timer0 que da una interrupción cada 4ms (4*4 = 16 ms, mas 4ms para las comunicaciones i2c = 20ms, que es el ciclo completo de los servos).

//
// Título: Generador de señales PWM a 50Hz con interfaz I2C
//         para control de servos
//
// Autor: Miguel Angel Ajo Pelayo <miguelangel@ajo.es>
//
// Fecha: 26/Septiembre/2007
//
// Compilador: CCS PCM versión 3.249
// Licencia: GPL
//

#include <16F689.h>
#device adc=10
#FUSES NOWDT,PUT,INTRC_IO,NOCPD,NOPROTECT,NOBROWNOUT,MCLR
#use delay(clock=8000000)
#use i2c(Slave,Fast,sda=PIN_B4,scl=PIN_B6,force_hw,address=0xC0)


#include "crc8.c"



#define PWM_RTS   PIN_C7
#define PWM0      PIN_A2
#define PWM1      PIN_C0
#define PWM2      PIN_C1
#define PWM3      PIN_C2
#define PWM4      PIN_C6
#define PWM5      PIN_C3
#define PWM6      PIN_C4
#define PWM7      PIN_C5

// portC all outputs
#define PORTC_TRIS 0b00000000
// portA A2 as output
#define PORTA_TRIS 0b11111011

#use fast_io(A)
#use fast_io(C)

#define VCC_SERVO_CHANNEL 3
#define VCC_CHANNEL       11



#define MIN_PWM_USECS 500
#define MAX_PWM_USECS 1800

volatile int cycle_sync = 0;
#int_RTCC
RTCC_isr()
{
   cycle_sync = 1;
}


void sync_to_4ms()
{
   while (cycle_sync ==0 );
   cycle_sync = 0;

}



BYTE address, buffer[0x20];

#INT_SSP
void ssp_interupt ()
{
   BYTE incoming, state;

    state = i2c_isr_state();

    if(state < 0x80)                            //Master is sending data
    {
        incoming = i2c_read();
        if(state == 1)                            //First received byte is address
            address = incoming;
        if(state>= 2)                            //Second received byte is data
            buffer[address++] = incoming;
    }
    if(state >= 0x80)                            //Master is requesting data
    {
        i2c_write(buffer[address++]);
    }

   address = address & 0xF;
}

// MMAP
//
// addr  /  type
//
//  00      PWM0
//  01      PWM1
//  02      PWM2
//  03      PWM3
//  04      PWM4
//  05      PWM5
//  06      PWM6
//  07      PWM7
//  08      PWM_ENABLE
//  09      PWM_CRC8
//  0A      VCC_SERVO_VALUE (HIGH)
//  0B      VCC_SERVO_VALUE (LOW)
//  0C      VCC_VALUE (HIGH)
//  0D      VCC_VALUE (LOW)
//  0E      OSC_FINETUNE
//  00



void read_levels()
{
   unsigned int16 vcc_servo_raw,vcc_raw;

   set_adc_channel(VCC_SERVO_CHANNEL);
   vcc_servo_raw = read_adc(ADC_START_AND_READ);

   set_adc_channel(VCC_CHANNEL);
   vcc_raw = read_adc(ADC_START_AND_READ);

   buffer[0xA] = MAKE8(vcc_servo_raw,1);
   buffer[0xB] = MAKE8(vcc_servo_raw,0);

   buffer[0xC] = MAKE8(vcc_raw,1);
   buffer[0xD] = MAKE8(vcc_raw,0);

}

static byte pwmA_counter;
static byte pwmB_counter;
static byte out_counter;
static byte pwm_enable_mask;

void cycle0_handle_comms()
{
   read_levels();

//   enable_interrupts(INT_SSP);
   output_high(PWM_RTS);   //signal that we are in handle_comms state
   delay_cycles(1);
   output_low(PWM_RTS);

   delay_ms(3);

//   disable_interrupts(INT_SSP);

}





void cycle1_pwm0_pwm1()
{

   pwmA_counter = buffer[0]+1;
   pwmB_counter = buffer[1]+1;


   out_counter = 0;

   if (pwm_enable_mask&0x01) output_high(PWM0);
   if (pwm_enable_mask&0x02) output_high(PWM1);

   delay_us (MIN_PWM_USECS);

#asm
C1_LOOP:
   DECF pwmA_counter
   BTFSC 03,2      // STATUS,Z
#endasm
   output_low(PWM0);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   DECF pwmB_counter
   BTFSC 03,2     // STATUS ,Z
#endasm
   output_low(PWM1);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   NOP
   NOP
   
   BTFSS out_counter,1     // salir cuando out_counter == 2
   GOTO C1_LOOP

#endasm


   output_low(PWM0);
   output_low(PWM1);

}


void cycle2_pwm2_pwm3()
{

   pwmA_counter = buffer[2]+1;
   pwmB_counter = buffer[3]+1;



   out_counter = 0;

   if (pwm_enable_mask&0x04) output_high(PWM2);
   if (pwm_enable_mask&0x08) output_high(PWM3);

   delay_us (MIN_PWM_USECS);

#asm
C1_LOOP:
   DECF pwmA_counter
   BTFSC 03,2      // STATUS,Z
#endasm
   output_low(PWM2);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   DECF pwmB_counter
   BTFSC 03,2     // STATUS ,Z
#endasm
   output_low(PWM3);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   NOP
   NOP

   BTFSS out_counter,1     // salir cuando out_counter == 2
   GOTO C1_LOOP

#endasm


   output_low(PWM2);
   output_low(PWM3);
}



void cycle3_pwm4_pwm5()
{


   pwmA_counter = buffer[4]+1;
   pwmB_counter = buffer[5]+1;


   out_counter = 0;

   if (pwm_enable_mask&0x10) output_high(PWM4);
   if (pwm_enable_mask&0x20) output_high(PWM5);

   delay_us (MIN_PWM_USECS);

#asm
C1_LOOP:
   DECF pwmA_counter
   BTFSC 03,2      // STATUS,Z
#endasm
   output_low(PWM4);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   DECF pwmB_counter
   BTFSC 03,2     // STATUS ,Z
#endasm
   output_low(PWM5);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   NOP
   NOP

   BTFSS out_counter,1     // salir cuando out_counter == 2
   GOTO C1_LOOP

#endasm


   output_low(PWM4);
   output_low(PWM5);


}



void cycle4_pwm6_pwm7()
{

   pwmA_counter = buffer[6]+1;
   pwmB_counter = buffer[7]+1;


   out_counter = 0;

   if (pwm_enable_mask&0x40) output_high(PWM6);
   if (pwm_enable_mask&0x80) output_high(PWM7);

   delay_us (MIN_PWM_USECS);

#asm
C1_LOOP:
   DECF pwmA_counter
   BTFSC 03,2      // STATUS,Z
#endasm
   output_low(PWM6);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   DECF pwmB_counter
   BTFSC 03,2     // STATUS ,Z
#endasm
   output_low(PWM7);
#asm
   BTFSC 03,2     // STATUS,Z
   INCF out_counter

   NOP
   NOP

   BTFSS out_counter,1     // salir cuando out_counter == 2
   GOTO C1_LOOP

#endasm


   output_low(PWM6);
   output_low(PWM7);
}

int osc_finetune =31;

void init()
{
   setup_adc_ports(sAN3|sAN11|VSS_VDD);
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_32);  // cada 4 ms
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);    // resolución de 2uS
   setup_comparator(NC_NC_NC_NC);
   //setup_vref(FALSE);

   buffer[0xE] = osc_finetune;
   setup_oscillator(OSC_8MHZ,31);


   set_tris_c(PORTC_TRIS);
   set_tris_a(PORTA_TRIS);

   enable_interrupts(INT_RTCC);
   enable_interrupts(INT_SSP);
   enable_interrupts(GLOBAL);

}


void main()
{

   int i;

   init();

   output_low(PWM_RTS);

   buffer[8] = 0x00;

   for (i=0;i<8;i++) buffer[i]=100;

   while(1)
   {
      sync_to_4ms();             //0ms
      cycle0_handle_comms();


      sync_to_4ms();
      pwm_enable_mask = buffer[8] ; // 4ms
      if (buffer[0xE]!=osc_finetune) 
      {
         osc_finetune = buffer[0xE];
         setup_oscillator(OSC_8MHZ,osc_finetune);
      }
      
      cycle1_pwm0_pwm1();

      sync_to_4ms();             //8ms
      cycle2_pwm2_pwm3();


      sync_to_4ms();             //12ms
      cycle3_pwm4_pwm5();

      sync_to_4ms();             //16ms
      cycle4_pwm6_pwm7();


   }
}

Espero que te sirva de referencia, ya me contarás :slight_smile:


#8

Hola:

Te quedo muy bien tu trabajo, y queria preguntarte si tuviste problemas al conectar los 8 servos de manera simultanea ya que yo compre una tarjeta controladora de servos SD21 (hasta 21 servos)pero al conectar mas de 6 servos Vigor VS2, el voltaje irremediablemente se cae, lo que he podido notar de esta tarjeta es que al conectarse la tarjeta SD21 generá automáticamente la señal para todos los servos se pongan en su posición central, y ya he checado con el osciloscopio y sigue generando la señal pero ahora a 2.2V. Me gustaría me comentaras si tu tuviste ese problema con tu tarjeta y si es el caso como lo solucionaste.

Saludos


#9

Que motores servo utilizaste?
Gracias anticipadas por la respuesta.:slight_smile: