Nous avons essayé de faire une balise capable de calculer ses propres déplacements sur un terrain plat. Son but était de calculer les variations de coordonnée du robot sur des temps relativement court. Pour cela, nous avons utilisons des accéléromètre 2 axes, placé à une distance fixe l'un de l'autre. Théoriquement il est donc possible de connaitre la variation de coordonnée et d'attitude (angle du robot par rapport à un bord du terrain) du robot. Le problème c'est qu'un sol parfaitement plat n'existe pas. Même avec la procédure d'étalonnage, il suffit que l'inclinaison du terrain varie de 1/10eme de degrés, pour que les calculs divergent.
La solution présentée ici n'est donc pas fiable. Mais c'est une base permettant de développer un système avec des accéléromètres 3 axes.
Il est en effet important d'avoir des accelerométres 3axes afin de pouvoir enlever la composante de gravitation universelle. Une fois celle ci éliminé, le système devient assez fiable et permet d'avoir des mesures avec une précision de 1% théoriquement.
Avec le système 2 axes, sur un terrain le plus plat possible, nous obtenions une précision de 5%. Mais la gravitation faussait bien entendu les résultats.
La Méthode1, permet de lire un total de 4 axes. Etant donnée la rapidité du microcontrôleur, un algorithme de détection de front montant et décendant suffit. Cependant, la méthode 2 illustre l'utilisation des Captures du pic.
Enfin, nous avons utilisé un module USB afin de pouvoir transféré les résultats rapidement, sans perdre d'information. Si la vitesse du quartz est assez élevé (voir datasheet), il est possible de transmettre jusqu'a un 1Mbps.
Methode1 :
//--------------------------------------------------------------
//--------------------------------------------------------------
// ACELEROMETRE 4 AXES BY BEN
// Version 1.0.1 du 28 avril 2006
// VITESSE a 16Mips
//--------------------------------------------------------------
//--------------------------------------------------------------
//
//Le code suivant detecte les fronts montants et d�endant d'un signal
//de type PWM, g���par les acc��om�res doubles axes.
//Dans cette version, les captures ne sont pas utilis�
//
#include <p30f2010.h>
#include <uart.h>
#include <stdio.h>
#include <stdlib.h>
_FOSC(CSW_FSCM_OFF & XT_PLL16); //XT with 16xPLL oscillator, Failsafe clock off
_FWDT(WDT_OFF); //Turn off the Watch-Dog Timer.
_FBORPOR(MCLR_EN & PWRT_OFF); //Enable MCLR reset pin and turn off the
//power-up timers.
_FGS(CODE_PROT_OFF); //Disable Code Protection
//----------------------------------------------------------------------------
// VARIABLE
//----------------------------------------------------------------------------
char buffer[16]; // Buffer com serie
//Variable Port Serie
#define FCY 16000000
#define BAUDRATE 57600
unsigned int i = 0;
unsigned int j = 0;
unsigned int k = 0;
unsigned char Digit1 = 0;
unsigned char Digit2 = 0;
unsigned char Digit3 = 0;
unsigned char Digit4 = 0;
unsigned char Digit5 = 0;
//variables d'acceleration
unsigned char front = 0;
unsigned int TimeL[2];
unsigned int TimeH[2];
signed int buffA = 0;
unsigned int T1[4];
unsigned int T2 = 0;
unsigned int buf[16];
//----------------------------------------------------------------------------
// INITIALISATION PORTS
//----------------------------------------------------------------------------
void inipic(void)
{
OSCCON = 0b0011001101000000; //Config Oscillateur...
TRISC = 24575; // Initialisation des ports
TRISE = 3; PORTE = 0; // RE0-8: input
}
//----------------------------------------------------------------------------
// INIT TIMER
//----------------------------------------------------------------------------
void init_timer(void)
{
T1CONbits.TGATE = 0;
T1CONbits.TCKPS = 0b01; //Prescaler
T1CONbits.TCS = 0; //Selectione l'oscilateur du pic
PR1 = 60000; //Valeur Max
T1CONbits.TON = 0; //Demarrer/Eteindre le timer
}
//----------------------------------------------------------------------------
// INIT PORT SERIE
//----------------------------------------------------------------------------
void init_serie(void)
{
unsigned int baudvalue;
unsigned int U1MODEvalue;
unsigned int U1STAvalue;
ConfigIntUART1(UART_RX_INT_DIS & UART_TX_INT_DIS );
U1MODEvalue = UART_EN & UART_IDLE_CON & UART_ALTRX_ALTTX &
UART_DIS_WAKE & UART_DIS_LOOPBACK &
UART_DIS_ABAUD & UART_NO_PAR_8BIT &
UART_1STOPBIT;
U1STAvalue = UART_INT_TX_BUF_EMPTY &
UART_TX_PIN_NORMAL &
UART_TX_ENABLE & UART_INT_RX_CHAR &
UART_ADR_DETECT_EN &
UART_RX_OVERRUN_CLEAR;
baudvalue = ((FCY/16)/BAUDRATE) - 1;
OpenUART1(U1MODEvalue, U1STAvalue, baudvalue);
}
//----------------------------------------------------------------------------
// COM SERIE
//----------------------------------------------------------------------------
void putsUSART_rom(char *data) // Envoi d'une chaine de caractere sur serie
{
//Envoie
do
{
while (BusyUART1());
putcUART1(*data);
}
while (*data++);
}
//----------------------------------------------------------------------------
// ENVOYER CHIFFRE PAR PORT SERIE
//----------------------------------------------------------------------------
void SendData(signed int mynumber)
{
// Ecris sur le port le mot
//Premiere variable
Digit1 = 0; Digit2 = 0; Digit3 = 0; Digit4 = 0;
//Le signe
if (mynumber < 0) { buffer[0]=0x2D; mynumber = -mynumber; }
else { buffer[0]=0x2B; }
//Les chiffres
while (mynumber>9999){Digit1++; mynumber = mynumber-10000;}
while (mynumber>999) {Digit2++; mynumber = mynumber-1000;}
while (mynumber>99) {Digit3++; mynumber = mynumber-100;}
while (mynumber>9) {Digit4++; mynumber = mynumber-10;}
Digit5 = mynumber;
buffer[1] = Digit1+0x30;
buffer[2] = Digit2+0x30;
buffer[3] = Digit3+0x30;
buffer[4] = Digit4+0x30;
buffer[5] = Digit5+0x30;
putsUSART_rom(buffer);
}
//----------------------------------------------------------------------------
// INITIALISATION DE T1 SUR CHAQUES AXES
//----------------------------------------------------------------------------
void initT1_E0()
{ signed long T1buff =0;
//Effectue 5 messures
for (i=0;i<16;i++)
{
while (!PORTEbits.RE0); // Attend le front montant
T1CONbits.TON = 0;TMR1 = 0;T1CONbits.TON = 1; //Amorce le Timer1
while (PORTEbits.RE0){buf[i] = TMR1;} //Attend le front d�endant
T1buff = T1buff + (signed long) buf[i];
}
T1[0] = T1buff/16; //D�ermine T1
T1[0] = (buf[1]+buf[2])/4 +(buf[3]+buf[4])/4;
}
void initT1_E1()
{ //Effectue 5 messures
for (i=0;i<16;i++)
{
while (!PORTEbits.RE1); // Attend le front montant
T1CONbits.TON = 0;TMR1 = 0;T1CONbits.TON = 1; //Amorce le Timer1
while (PORTEbits.RE1){buf[i] = TMR1;} //Attend le front d�endant
}
T1[1] = (buf[1]+buf[2])/4 +(buf[3]+buf[4])/4; //D�ermine T1
}
//----------------------------------------------------------------------------
// INITIALISATION DE T2 SUR LES 2 ACCELERO
//----------------------------------------------------------------------------
void initT2(void)
{ //Effectue 5 messures
for (i=0;i<5;i++)
{
while (!PORTEbits.RE0) {buf[i] = TMR1;} // Attend le front montant
T1CONbits.TON = 0;TMR1 = 0;T1CONbits.TON = 1; //Amorce le Timer1
while (PORTEbits.RE0); //Attend le front d�endant
}
T2 = (buf[1]+buf[2])/4 +(buf[3]+buf[4])/4; //D�ermine T2
}
//----------------------------------------------------------------------------
// DETECTION DES FRONTS
//----------------------------------------------------------------------------
void detection(void)
{
//Detection du premier front montant avant de lancer le timer
while (!PORTE);
T1CONbits.TON = 1;
//Detection des fronts
front = 0;
TimeH[0] =0; TimeL[0] = 0;
TimeH[1] =0; TimeL[1] = 0;
while (front != 3)
{
//Detection des fronts montants
if ((PORTEbits.RE0 == 1) && (TimeH[0] == 0)) {TimeH[0] = TMR1; front = front+1;}
//Detection des fronts decendant
if ((PORTEbits.RE0 == 0) && (TimeL[0] == 0) && (TimeH[0] !=0)) {TimeL[0] = TMR1;front = front+1;}
if ((PORTEbits.RE1 == 0) && (TimeL[1] == 0) && (TimeH[0] !=0)) {TimeL[1] = TMR1;front = front+1;}
}
}
//----------------------------------------------------------------------------
// MAIN
//----------------------------------------------------------------------------
int main(void)
{
//Initialisation PIC
inipic();
init_serie();
init_timer();
//Initialisation Accelero
initT1_E0();
initT1_E1();
initT2();
signed long mytest = 0; //2 miliards
signed long myfloat;
signed long A0 =0;
signed long V0 =0;
signed long X0 =0;
signed long A1 =0;
signed long V1 =0;
signed long X1 =0;
//Lancement boucle principale
while (1)
{
//Messures des 8 fronts
detection();
//Calcul apr� que le dernier signal soit red�endu �0
//Acceleration: 500 = 9,80m/s
//Accelero1:
buffA = TimeL[0] - TimeH[0] - T1[0];
buffA = buffA * 2 ;
A0 = (signed long) buffA;
X0 = X0 + A0 + V0; //Calcul position
V0 = 2*A0+V0; //Calcul de la vitesse
//Accelero2:
buffA = TimeL[1] - TimeH[0] - T1[1];
buffA = buffA * 2 ;
A1 = (signed long) buffA;
X1 = X1 + A1 + V1; //Calcul position
V1 = 2*A1+V1; //Calcul de la vitesse
//Envoi des r�ultats
k++;
if ( k == 200)
{ k = 0;
mytest = X0/10000;
buffA = (signed int) mytest;
SendData(buffA);
mytest = X1/10000;
buffA = (signed int) mytest;
SendData(buffA);
X0 = 0; V0 = 0;
X1 = 0; V1 = 0;
}
//Eteint le timer
T1CONbits.TON = 0;
TMR1 = 1;
}
}
//---------------------------------------------------------------------------
Pour toutes questions: benjamin.baque AT polymtl.ca
|
