PIC: Servomotores

Neste post iremos controlar 8 servomotores utilizando o PIC16F628A.

O que é um servomotor?

Servomotor é uma máquina, mecânica ou eletromecânica, que apresenta um giro proporcional a um comando, em vez ficar girando livremente. Servomotores recebem um sinal de controle, verificam a posição atual e atuam no sistema indo para a posição desejada.

O fio de controle é usada para informar o ângulo. O ângulo é determinado pela duração de um pulso que é aplicada ao fio de controle. O servo processa um pulso a cada 20 ms. O comprimento do pulso determina a distância que o motor gira. Um pulso de 1.5 ms, por exemplo, vai fazer o motor parar na posição de 90 graus. Se o pulso é menor do que 1.5 ms, então o motor irá girar o eixo para mais próximo de 0 graus. Se o impulso for mais longo do que 1.5 ms, o eixo gira mais perto de 180 graus.

Veja que os tempos do pulso variam para cada modelo de servomotor.

CÓDIGO

Sabemos que o período completo para um servo é de 20 ms. Para utilizar 8 servos, precisamos de um tempo de 20/8 = 2.5 ms. Dentro desse tempo, iremos controlar um servo de cada vez, um após o outro. Veja:

Com a ajuda do Timer1, pode-se gerar o sinal para cada servo. Vale lembrar que, quanto maior o clock, menor será o erro.

Para um clock de 16MHz, timer1 configurado como contador de 16 bits e prescaler 1:4, temos que cada tick( ou pulso ) do contador possui 1us. Se, por exemplo, queremos um tempo de 1.5 ms ( nível alto ), simplesmente faremos o timer1 contar 1500 ticks, ou seja, carregar o timer1 com 65535 - 1500 = 64035. O tempo restante será 1 ms ( nível baixo ). Então devemos carregar novamente o timer1 com 65535 - 1000 = 64535.

Resumo: Definimos nível logico alto para um pino, carregamos o timer1 com o valor 64035. Espere o estouro do timer1. Define nível lógico baixo para um pino, carregamos o timer1 com 64535. Espere o estouro do timer1. Repita o ciclo para um outro servo.

BIBLIOTECA PARA PIC16F

MikroC PRO PIC

//Numero maximo de servos
const N_SERVOS = 8;

//Tick = 4 / clock(16MHz) * Prescaler
//Tick = 4 / 16 * 4 = 1us
const unsigned TicksFrame =  2500;   // 20 ms / 8 servos / Tick
const unsigned TicksMin = 1000;      // posicao 0º = 1 ms / Tick
const unsigned TicksCenter =  1500;  // posicao 90º = 1.5 ms / Tick
const unsigned TicksMax = 2000;      // posicao 180º = 2 ms / Tick

unsigned Timer1 at TMR1L;

//Registro de configuracao para cada servo
typedef struct
{
    char Port;
    char Pino;
    unsigned Angulo;
    union
    {
       char Enable:1;
    };
}Servos;

//cria os 8 servos
Servos myServos[N_SERVOS];

static char flags = 0;
    sbit pulso at flags.B0;

//Inicializa os servos
void Servo_Init()
{
char i;
static char temp = 0;
     for( i = 0; i < N_SERVOS; i++ )
     {
         myServos[i].Port = &temp;
         myServos[i].Pino = 0;
         myServos[i].Angulo = TicksMin;
         myServos[i].Enable = 0;
     }
     
     Timer1 = 65535 - TicksFrame;
     T1CON = 0b00100001; //Prescaler 1:4
     TMR1IE_bit = 1;
     PEIE_Bit = 1;
     GIE_Bit = 1;
}

//Adiciona um novo servomotor
void Servo_Attach( char servo, char out, char pin )
{
Servos *ptr;
      if( servo > N_SERVOS )
         return;
      ptr = &myServos[servo];
      (*ptr).Port = out;
      (*ptr).Pino = 1 << pin;
      (*ptr).Enable = 1;
}

void Servo_Interrupt()
{
static char servo_idx = 0;
char port, pino;
unsigned an;

        //Passa o valor do endereço para o ponteiro
        FSR = (char)&myServos[servo_idx];

        port = INDF; //Recebe o valor da porta, myServos[x].Port
        FSR++;       //Incrementa o ponteiro
        pino = INDF; //Recebe o valor do pino, myServos[x].Pino
        FSR++;       //Incrementa o ponteiro
        ((char*)&an)[0] = INDF;
        FSR++;
        ((char*)&an)[1] = INDF;//Recebe o angulo, myServos[x].Angulo
        FSR++;
        
        //Servo habilitado?
        if( INDF.B0 ) //myServos[x].enable
        {
            FSR = port; //Passa para o ponteiro o endereço da porta

            if( !pulso )
            {
              Timer1 = 65535 - an + 29;//(29) - fator de correção?
              INDF |= pino;
            }
            else
            {
              Timer1 = (65535 - TicksFrame) + an + 37;
              INDF &= ~pino;
              ++servo_idx &= 7;
            }

            pulso = ~pulso;
        }
        else
        {
            Timer1 = (65535 - TicksFrame);
            ++servo_idx &= 7;
        }
}

BIBLIOTECA PARA PIC18F

MikroC PRO PIC

//Numero maximo de servos
const N_SERVOS = 8;

//Tick = 4 / clock(16MHz) * Prescaler
//Tick = 4 / 16 * 4 = 1us
const unsigned TicksFrame =  2500;   // 20 ms / 8 servos / Tick
const unsigned TicksMin = 1000;      // posicao 0º = 1 ms / Tick
const unsigned TicksCenter =  1500;  // posicao 90º = 1.5 ms / Tick
const unsigned TicksMax = 2000;      // posicao 180º = 2 ms / Tick

unsigned Timer1 at TMR1L;
//Registro de configuracao para cada servo

typedef struct
{
    char Port;
    char Pino;
    unsigned Angulo;
    union
    {
       char Enable:1;
    };
}Servos;

//cria os 8 servos
Servos myServos[N_SERVOS];

static char flags = 0;
    sbit pulso at flags.B0;

//Inicializa os servos
void Servo_Init()
{
char i;
Servos *Ptr;
     for( i = 0; i < N_SERVOS; i++ )
     {
         Ptr = &myServos[i];
         (*Ptr).Port = 0;
         (*Ptr).Pino = 0;
         (*Ptr).Angulo = TicksMin;
         (*Ptr).Enable = 0;
     }

     Timer1 = 65535 - TicksFrame;
     T1CON = 0b00100001; //Prescaler 1:4
     TMR1IE_bit = 1;
     PEIE_Bit = 1;
     GIE_Bit = 1;
}

//Adiciona um novo servomotor
void Servo_Attach( char servo, char out, char pin )
{
Servos *ptr;
      if( servo > N_SERVOS )
         return;
      ptr = &myServos[servo];
      (*ptr).Port = out;
      (*ptr).Pino = 1 << pin;
      (*ptr).Enable = 1;
}

void Servo_Interrupt()
{
static char servo_idx = 0;
char port, pino;
unsigned an;

        //Passa o valor do endereço para o ponteiro
        *(unsigned*)&FSR0L = &myServos[servo_idx];
        port = POSTINC0; //Recebe o valor da porta, myServos[x].Port
        pino = POSTINC0; //Recebe o valor do pino, myServos[x].Pino
        ((char*)&an)[0] = POSTINC0;
        ((char*)&an)[1] = POSTINC0;//Recebe o angulo, myServos[x].Angulo
        
        //Servo habilitado?
        if( INDF0.B0 ) //myServos[x].enable
        {
            FSR0H = 0x0F;
            FSR0L = port; //Passa para o ponteiro o endereço da porta
            if( !pulso )
            {
              Timer1 = 65535 - an + 29;//(29) - fator de correção?
              INDF0 |= pino;
            }
            else
            {
              Timer1 = (65535 - TicksFrame) + an + 37;
              INDF0 &= ~pino;
              ++servo_idx &= 7;
            }
            pulso = ~pulso;
        }
        else
        {
            Timer1 = (65535 - TicksFrame);
            ++servo_idx &= 7;
        }
}

EXEMPLO:

//Copie e cole a biblioteca aqui!!!

void interrupt()
{
    if( TMR1IF_Bit )
    {
       Servo_Interrupt();
       TMR1IF_Bit = 0;
    }
}

void main()
{
      //ADCON1 = 0x07; //Desativa os canais analogicos( PIC18F4550 )
      TRISB = 0; //Define os pinos da PORTB como saída
      
      Servo_Init();
      Servo_Attach( 0, (char)&PORTB, 0 );
      Servo_Attach( 1, (char)&PORTB, 1 );
      Servo_Attach( 2, (char)&PORTB, 2 );
      Servo_Attach( 3, (char)&PORTB, 3 );
      Servo_Attach( 4, (char)&PORTB, 4 );
      Servo_Attach( 5, (char)&PORTB, 5 );
      Servo_Attach( 6, (char)&PORTB, 6 );
      Servo_Attach( 7, (char)&PORTB, 7 );

      myServos[0].Angulo = 544;
      myServos[1].Angulo = 1500;
      myServos[2].Angulo = 2000;
      myServos[3].Angulo = 1900;
      myServos[4].Angulo = 1007;
      myServos[5].Angulo = 2009;
      myServos[6].Angulo = 1890;
      myServos[7].Angulo = 1501;


      while(1)
      {

      }
}