Mixer para gimbal com resolução de 5000 pontos

[Antonio Garcia]

Projeto de minha autoria.

Feito com PIC12F675. Depois posto o diagrama.

[img:9cf4a0b3f1]http://i1313.photobucket.com/albums/t555/agf001/IMG_1107_mod_zpsda4fdffb.jpg[/img:9cf4a0b3f1]

[code:1:9cf4a0b3f1]/*

Mixer para gimbal com servos. Entrada: os dois sinais da DJI NAZA, Saida: dois sinais mixados para os servos.

OBSERVAÇÃO: No programa de gravação do PIC (ic-prog etc),
escovar os bytes de 0x23FF (equivalente a CALL 0x3FF) para 0x0000 (equivalente a NOP).
Esses bytes ficam localizados por volta do endereço 0x00C do código de máquina.
Essa chamada serve para carregar os valores de calibração do oscilador interno.
Se estiverem corrompidos, o PIC não funciona. Então o procedimento acima anula a chamada a essa rotina.

*/

#pragma psect text=_main_

#define MAIN

#include <htc.h>
#include <math.h>
#include <string.h>

__CONFIG ( UNPROTECT & BORDIS & MCLRDIS & PWRTEN & WDTDIS & HS );

#define CLOCK 20u // MHz

// Larguras dos pulsos dos servos em quantidades de 200ns
#define MAX (2500u*CLOCK/4u)
#define MED (1500u*CLOCK/4u)
#define MIN (500u*CLOCK/4u)

#define IOCE1 IOCB2
#define IOCE2 IOCB3
#define ENTR1 GPIO2
#define ENTR2 GPIO3
#define SAIDA1 GPIO1
#define SAIDA2 GPIO0

/* Leitura das saídas da NAZA */
unsigned int E1, /* entrada 1 */
E2 /* entrada 2 */;

// Variáveis auxiliares para os cálculos
int aux;

/* Resutlado a ser aplicado nas saídas do mixer */
unsigned int S1, /* saída 1 */
S2, /* saída 2 */
s;

/* controle do fluxo de execução */
char estado;

void interrupt intrr(void)
{
if (TMR1IF)
{
TMR1IE = 0;
TMR1IF = 0;
TMR1ON = 0;

switch (estado)
{
case 1:
// Finaliza geração de S1
SAIDA1 = 0;
estado = 2; // prepara para gerar S2
break;
case 3:
// Finaliza geração de S2
SAIDA2 = 0;
estado = 4; // prepara para aguardar leitura da ENTRADA de sinal
break;
}

return;
}
else

if (GPIF)
{
contLED++;

switch (estado)
{
case 5:
if (ENTR1)
TMR1ON = 1;
else
{
TMR1ON = 0;
E1 = (TMR1H<<8)|TMR1L;
IOCE1 = 0;
GPIE = 0; // não permite mais interrupções por mudança nas entradas
estado = 9;
}
break;

case 7:
if (ENTR2)
TMR1ON = 1;
else
{
TMR1ON = 0;
E2 = (TMR1H<<8)|TMR1L;
IOCE2 = 0;
GPIE = 0; // não permite mais interrupções por mudança nas entradas
estado = 10;
}
break;
}
GPIF = 0;
return;
}
}


void main(void)
{
CM2 = CM1 = CM0 = 1; /* Comparadores de tensão desligados */
ANS2 = ANS1 = ANS0 = 0; /* Digital I/O */

TRISIO = 0b001111100;

T1CKPS1 = T1CKPS0 = 0; // Prescaler de timer1 1:1

PEIE = GIE = 1;

S1 = MED+1; // 1,5ms / 200ns
S2 = MED-1;

SAIDA1 = SAIDA2 = 0;

for (estado = 4;;)
{
switch (estado)
{
case 0:
// Gera sinal em S1
s = (unsigned int) 0xFFFF;
s -= S1;
s += 17; // Ajuste pelo tempo gasto na entrada da rotina de interrução
TMR1H = s >> 8;
TMR1L = s & 0x00FF;

TMR1IF = 0;
TMR1IE = 1;
TMR1ON = 1;

SAIDA1 = 1;

estado = 1;
break;

case 1:
// Aguardando finalizar geração de S1
break;

case 2:
// Gera sinal em S2
s = (unsigned int) 0xFFFF;
s -= S2;
s += 17; // Ajuste pelo tempo gasto na entrada da rotina de interrução
TMR1H = s >> 8;
TMR1L = s & 0x00FF;

TMR1IF = 0;
TMR1IE = 1;
TMR1ON = 1;

SAIDA2 = 1;

estado = 3;
break;

case 3:
// Aguardando finalizar geração de S2
break;

case 4:
// Aguarda interrupção de mudança de estado na ENTRADA1 de sinal
TMR1ON = 0;
TMR1H = TMR1L = 0;
TMR1IF = 0;
TMR1IE = 0;

IOCE1 = 1;
GPIF = 0;
GPIE = 1;
estado = 5;
break;

case 5:
// Aguardando interrupção na ENTRADA1, tanto de subida como de descida, para aquisição do primeiro pulso
break;

case 6:
// Aguarda interrupção de mudança de estado na ENTRADA2 de sinal
TMR1ON = 0;
TMR1H = TMR1L = 0;
TMR1IF = 0;
TMR1IE = 0;

IOCE2 = 1;
GPIF = 0;
GPIE = 1;
estado = 7;
break;

case 7:
// Aguardando interrupção na ENTRADA2, tanto de subida como de descida, para aquisição do segundo pulso
break;

case 8:
// Faz os calculos do mixer para gerar as saídas
if ( (E1 >= MIN) && (E1 <= MAX) && (E2 >= MIN) && (E2 <= MAX) )
{
S1 = (E1+E2)/2;
S2 = MED+((int)E1-(int)E2)/2;

// Amplia curso dos servos
aux = (int)S1-(int)MED;
aux = aux + aux/2; // aumenta curso do servo por um fator de 1,5
S1 = aux + MED;

aux = S2 - MED;
aux = aux + aux/2; // aumenta curso do servo por um fator de 1,5
S2 = aux+MED;

// Garante que sinais fiquem dentro dos limites suportados pelos servos
if (S1 > MAX) S1 = MAX;
if (S1 < MIN) S1 = MIN;
if (S2 > MAX) S2 = MAX;
if (S2 < MIN) S2 = MIN;
}
estado = 0; // Vai para geração dos pulsos de saída.
break;

case 9:
// Após ler ENTR1, aguarda ambas as entradas ierm para zero
if (ENTR1 || ENTR2)
estado = 9;
else
estado = 6;
break;

case 10:
// Após ler ENTR2, aguarda ambas as entradas ierm para zero
if (ENTR1 || ENTR2)
estado = 10;
else
estado = 8;
break;
} // switch

} // for(;;)
}
[/code:1:9cf4a0b3f1]