Fork me on GitHub

03/03/14

PIC: Servomotores

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)
      {

      }
}


21 comentários:

  1. Ótimo site, estou um bom tempo procurando por isso, mas como ficaria para um cristal de 20MHz

    ResponderExcluir
    Respostas
    1. Com um clock de 20MHz o valor do Tick não será igual a 1us. será 0.8us. Entao voce terá que ajustar o valor dos tempos dos angulos: T = TempoDoAngulo / 0.8.
      Altere tambem o valor das constantes definidas no topo do código.

      Excluir
  2. Tiago como seria o codigo fonte se fosse para controlar apenas um servo_motor? preciso para fazer um projecto muito especial.

    calembe marcos antonio dos santos

    ResponderExcluir
    Respostas
    1. Deixe apenas:
      Servo_Attach( 0, (char)&PORTB, 0 ); // assim ativa somente o pino RB0. altere o pino se desejar

      Para alterar o angulo use:
      myServos[0].Angulo = 544; //Esta em (ms)

      Excluir
  3. Bom dia, qual é esse clock utilizado, n achei na biblioteca

    ResponderExcluir
    Respostas
    1. esse clock é o clock do projeto, isto é, o clock do cristal oscilador.

      Excluir
  4. muito obrigado Tiago Henrique. me ajudaste muito cara!! valeu. gosto muito do seu blog

    ResponderExcluir
  5. Tiago Henrique concernente ao servo motor: eu preciso que me ajudas a fazer o seguinte: usar um botão para trocar os estado de rotação do servo motor. por exemplo: enquanto o botão estiver no nivel baixo o servo mantem no seu estado normal ou seja faz um angulo de zero grau; caso o botão estiver em alto o servo gira para 90 grau. como fica o codigo nesse caso?

    ResponderExcluir
    Respostas
    1. Primeirante tente descobrir o tempo de pulso necessario para seu servo-motor realizar os angulos de 0° e 90°.

      O código da funcao principal:
      void main()
      {
      //ADCON1 = 0x07; //Desativa os canais analogicos( PIC18F4550 )
      TRISB.B0 = 0; // RB0 como saída
      TRISB.B1 = 1; //RB1 como entrada

      Servo_Init();
      Servo_Attach( 0, (char)&PORTB, 0 ); //adiciona um servo ao pino RB0
      myServos[0].Angulo = 544; //tempo de pulso para o angulo de 0°

      while(1)
      {
      //se RB1 for pressionado o tempo = 1422(90°), caso contrario tempo = 544(0°)
      myServos[0].Angulo = PORTB.B1 ? 1422: 544;
      }

      Excluir
  6. Como faço para usar esse código em um pic rodando o bootloader daqui do blog?

    ResponderExcluir
    Respostas
    1. Pensei, no caso, utilizar o oscilador interno pra base de tempo do timer 1. Isso funcionaria?

      Excluir
  7. - tem como eu alterar o angulo de apenas um servo?

    ResponderExcluir
    Respostas
    1. sim! é so ajustar a variavel myServos[0].Angulo = tempo_ms;

      Excluir
    2. - estava tentando mudar o angulo do servo usando um potenciômetro. Usando um outro pic pra fazer essa leitura e então enviar os dados via serial para outro pic rodando esse código acima, tentei varias formas mas não consegui. Tem como você me dar uma luz, nesse problema. Não quero que faça, só me de uma dica.

      Excluir
  8. Tiago es um maximo no ramo da programação.... valeu obrigado pela ajuda

    ResponderExcluir
  9. Tiago preciso fazer um curso. bem profundo no ramo de programação de microcontrolador.. o que faço?
    eu tenho conhecimento mais preciso aprofundar mais..

    ResponderExcluir
  10. Como faço para o servo ir para outro ângulo sem ser 90º, 180º ou 0º. Ex quero que o servo vá para 127º

    ResponderExcluir
  11. Como faço para fazer uma sequencia de 6 servos, um após o outro, pois quero fazer um braço robótico.

    ResponderExcluir
  12. Boa Tarde Tiago...
    Estou com um problema, coloquei um botão para ir incrementando uma variável, e essa variável eu coloquei dentro da função myServos[0].Angulo = posicao;

    Só que de vez em quando o servo da tipo uns "fortes espasmos" na hora de incrementar e decrementar... Tem ideia doq possa ser? Esse estou utilizando os servos em meu TCC

    ResponderExcluir

Postagens Relacionadas!!