Fork me on GitHub

03/03/2014

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

      }
}


26 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. 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
  9. Como faço para fazer uma sequencia de 6 servos, um após o outro, pois quero fazer um braço robótico.

    ResponderExcluir
  10. 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
  11. Olá,toda essa programação ésimplesmente pra girar o servo a uma posição pré-determinada?

    ResponderExcluir
  12. Boa tarde Pessoal,
    Estou com com uma dificuldade enorme de fazer o pic 16f628A ler 3 Canais ppm do meu Radio.
    Eu sei que funciona, mas não estou sabendo como Fazer?
    Resumindo a historia, configurei os portb para interrupção externa.
    Mas me parece, que quando há a interrupção,ele não sabe em qual pino foi?
    Outra coisa que percebi: Vamos supor que houve uma interrupção em RB6 na borda de descida por exemplo.
    Quando ele entra na rotina para tratar a interrupção, se houver outra, ele vai tentar tratar a outra e fica doidão.
    Com isso dá pau na hora de contar o tempo da borda de subida.
    Tive que criar uma variavel que force a ler os canais por sequencia.
    O problema é que se um falhar, todos falham.
    Eu estava olhando o código dos servos, talvez a minha resposta esteja aqui.
    Alguem pode me indicar um livro ou curso de programação C avançado?
    Eu não manjo de Typedeff, struct preciso aprender e entender!
    O código não esta 100% igual ao que estou usando, pq estou no serviço e fiz de cabeça, mas está 99% completo.
    Se alguem puder me dar um Help, ficarei grato

    Na Rotina de interrupção fiz seguinte comando:

    #define Portb.Rb7 Canal1;
    #define Portb.Rb6 Canal2;
    #define Portb.Rb5 Canal3;
    long int Tempo1,Tempo2,Tempo3 = 0;
    char ControlaCanal = 1;
    char last_channe1l, last_channe2l,last_channe3l = 0
    void ZeraTimer()
    {
    Tmr1h = 0;
    Tmr1L = 0;
    }
    int CapturaTempo()
    {
    Return (Tmr1h<<+Tmr1l);
    }
    void interrupt();
    {
    IF (intcon.rbif == 1)
    {
    if (last_channe1 == 0 && Canal1 = 1 && ControlaCanal == 1)
    {
    zeraTimer();
    Last_Channel1 = 0;

    }
    else
    if (Last_Channel1 == 1 && Canal1 == 1 && ControlaCanal == 1)
    {
    Tempo1 = CapturaTempo();
    last_channe1l = 0;
    ControlaCanal = 2;
    trisb.rb7 = 0;
    trisb.rb5 = 0;
    trisb.rb6 = 1;
    }
    if (last_channe2 == 0 && Canal2 = 1 && ControlaCanal == 2)
    {
    zeraTimer();
    Last_Channel2= 0;

    }
    else
    if (Last_Channel2 == 1 && Canal2 == 1 && ControlaCanal == 2)
    {
    Tempo2 = CapturaTempo();
    last_channe2l = 0;
    ControlaCanal = 3;
    trisb.rb6 = 0;
    trisb.rb7 = 0;
    trisb.rb5 = 1;
    }
    if (last_channe3 == 0 && Canal3 = 1 && ControlaCanal == 3)
    {
    zeraTimer();
    Last_Channel3= 0;

    }
    else
    if (Last_Channel3 == 1 && Canal3 == 1 && ControlaCanal == 3)
    {
    Tempo2 = CapturaTempo();
    last_channe3l = 0;
    ControlaCanal = 1;
    trisb.rb5 = 0;
    trisb.rb6 = 0;
    trisb.rb7 = 1;
    }
    }

    }
    void main()
    {
    trisb.rb7 = 1;
    Canal1 = 0;

    trisb.rb6 = 0;
    Canal2 = 0;

    trisb.rb5 = 0;
    Canal3 = 0;
    while(1)
    {
    printf("Tempo1");
    printf("Tempo1");
    printf("Tempo1");
    delay_ms(200);
    }
    }

    ResponderExcluir
  13. Pessoal, retificando o código
    Errei no else dos canais
    Outra coisa, ao moderador se é possivel editar os posts?

    Código:

    #define Portb.Rb7 Canal1;
    #define Portb.Rb6 Canal2;
    #define Portb.Rb5 Canal3;
    long int Tempo1,Tempo2,Tempo3 = 0;
    char ControlaCanal = 1;
    char last_channe1l, last_channe2l,last_channe3l = 0
    void ZeraTimer()
    {
    Tmr1h = 0;
    Tmr1L = 0;
    }
    int CapturaTempo()
    {
    Return (Tmr1h<<+Tmr1l);
    }
    void interrupt();
    {
    IF (intcon.rbif == 1)
    {
    if (last_channe1 == 0 && Canal1 = 1 && ControlaCanal == 1)
    {
    zeraTimer();
    Last_Channel1 = 0;

    }
    else
    if (Last_Channel1 == 1 && Canal1 == 0 && ControlaCanal == 1)
    {
    Tempo1 = CapturaTempo();
    last_channe1l = 0;
    ControlaCanal = 2;
    trisb.rb7 = 0;
    trisb.rb5 = 0;
    trisb.rb6 = 1;
    }
    if (last_channe2 == 0 && Canal2 = 1 && ControlaCanal == 2)
    {
    zeraTimer();
    Last_Channel2= 0;

    }
    else
    if (Last_Channel2 == 1 && Canal2 == 0 && ControlaCanal == 2)
    {
    Tempo2 = CapturaTempo();
    last_channe2l = 0;
    ControlaCanal = 3;
    trisb.rb6 = 0;
    trisb.rb7 = 0;
    trisb.rb5 = 1;
    }
    if (last_channe3 == 0 && Canal3 = 1 && ControlaCanal == 3)
    {
    zeraTimer();
    Last_Channel3= 0;

    }
    else
    if (Last_Channel3 == 1 && Canal3 == 0 && ControlaCanal == 3)
    {
    Tempo2 = CapturaTempo();
    last_channe3l = 0;
    ControlaCanal = 1;
    trisb.rb5 = 0;
    trisb.rb6 = 0;
    trisb.rb7 = 1;
    }
    }

    }
    void main()
    {
    trisb.rb7 = 1;
    Canal1 = 0;

    trisb.rb6 = 0;
    Canal2 = 0;

    trisb.rb5 = 0;
    Canal3 = 0;
    while(1)
    {
    printf("Tempo1");
    printf("Tempo1");
    printf("Tempo1");
    delay_ms(200);
    }
    }

    ResponderExcluir
  14. Boa noite pessoal
    Tentei entender esse codigo dos servos mas não consegui.
    Será que alguem poderia explicar?
    Ou eu Tentei fazer um código onde eu incremento uma variavel de 0 a 7 e passando a variavel, trocaria a porta do portb, mas sem sucesso.
    Muito obrigado

    ResponderExcluir
    Respostas
    1. Tenho uma implementação, se quiser pode pegar
      https://github.com/andrmalta/ROPIC

      Excluir

Postagens Relacionadas!!