From ca20561f2ee33afcd43e7a0cb80b1361342f6df9 Mon Sep 17 00:00:00 2001 From: burg Date: Thu, 22 Jul 2004 22:05:48 +0000 Subject: Ajout de fichiers et modife des fichiers existant. Mesure possible --- n/accel/Makefile | 2 +- n/accel/README | 1 + n/accel/accel.c | 62 +++++++++++++++++++++++++++++++++++---------------- n/accel/calcul.c | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ n/accel/calcul.h | 53 +++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 166 insertions(+), 20 deletions(-) create mode 100644 n/accel/README create mode 100644 n/accel/calcul.c create mode 100644 n/accel/calcul.h (limited to 'n/accel') diff --git a/n/accel/Makefile b/n/accel/Makefile index 1c62445..618ee22 100644 --- a/n/accel/Makefile +++ b/n/accel/Makefile @@ -1,5 +1,5 @@ PROGS =accel -SOURCES = accel.c +SOURCES = accel.c calcul.c DOC = howto-avr.html EXTRACTDOC = MODULES = n/avr/proto n/avr/rs232 diff --git a/n/accel/README b/n/accel/README new file mode 100644 index 0000000..2872096 --- /dev/null +++ b/n/accel/README @@ -0,0 +1 @@ +Accelerometre diff --git a/n/accel/accel.c b/n/accel/accel.c index 51404c3..e9eab90 100644 --- a/n/accel/accel.c +++ b/n/accel/accel.c @@ -35,10 +35,12 @@ #include #include #include -#include "avrconfig.h" #include -#define DEBUG +#include "calcul.h" +#include "avrconfig.h" + +//#define DEBUG #ifdef DEBUG #define debug_putc(x) rs232_putc (x) @@ -50,19 +52,21 @@ * des données * */ -enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul }; -int16_t Tb,Tc,Td; +//enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul }; +volatile uint16_t Tb,Tc,Td; +uint16_t Tx,Ty,T2; +int16_t Gx,Gy; +uint16_t Timer1; /* Variable Globale */ -enum T_etat etat; +//volatile enum T_etat etat; /* Interruption input_capture */ SIGNAL(SIG_INPUT_CAPTURE1) { - rs232_putc('i'); //code de l'interruption switch (etat) { @@ -74,7 +78,7 @@ SIGNAL(SIG_INPUT_CAPTURE1) etat = rd_at_Tb; break; case rd_at_Tb : - //debug_putc('B'); + debug_putc('B'); Tb = ICR1; // sauvegarde du registre Input Capture etat = rd_at_Tc; TIMSK &= ~0x20; //désactive les interruptions Input Capture @@ -83,7 +87,7 @@ SIGNAL(SIG_INPUT_CAPTURE1) TIMSK |= 0x20; // Réactive les interruptions Input Capture break; case rd_at_Tc : - //debug_putc('C'); + debug_putc('C'); Tc = ICR1; // sauvegarde du registre Input Capture TIMSK &= ~0x20; //désactive les interruptions Input Capture TCCR1B &= ~_BV(ICES1); // trigge sur le front descendant @@ -99,7 +103,7 @@ SIGNAL(SIG_INPUT_CAPTURE1) TCCR1B |= _BV(ICES1); //trigge sur le front montant TIMSK |= 0x20; // Réactive les interruptions Input Capture break; - default : debug_putc('L'); + default :debug_putc('L'); break; // Ce cas ne doit pas arriver } } @@ -112,14 +116,29 @@ SIGNAL(SIG_OVERFLOW1) void test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) { - proto_send (c, argc, argv); + //proto_send (c, argc, argv); + switch (c) + { + case 'C' : proto_send0('C'); + calibration(); + break; + case 'X' : proto_send2('X',Tx >> 8, Tx); + break; + case 'Y' : proto_send2('Y',Ty >> 8, Tx); + break; + case 'T' : proto_send2('T',Timer1 >> 8, Timer1); + break; + case 'G' : proto_send2('x',Gx >> 8, Gx); + proto_send2('y',Gy >> 8, Gy); + break; + default : proto_send1('E',2); //erreur 2 Command incorrect + } + } int main (void) { - int16_t Tx,Ty,T2,T2_old; - //initialisation rs232_init(); proto_init(test_callback,rs232_putc); @@ -128,7 +147,8 @@ main (void) //initialisation du timer TCCR1B = _BV(ICES1) | _BV(CS10); - TIMSK = _BV(TICIE1); // Input Capture Interrupt enable + // Input Capture Interrupt enable & Timer 1 overflow + TIMSK = _BV(TICIE1) | _BV(TOIE1); sei (); //Boucle while(1) @@ -140,14 +160,18 @@ main (void) // calcul des Tons if (etat == calcul) { - Tx = Tb; // Calcul des Ton + + Tx = Tb; // Calcul des Ton Ty = Td - Tc; // Todo T2 ne doit pas être calculer tout le temps - T2 = (Td - ((Td - Tc)/2)) - Tb/2; //Calcul de T2 - T2_old =T2; - //proto_send4 ('M', Tx >> 8, Tx, Ty >> 8, Ty); // envoie les 2 temps Ton - etat = strt_at_Ta; //remise dans l'état de capture - proto_send2 ('N', T2_old >>8, T2_old); // envoie la valeur T2 + T2 = (Td - (Ty/2)) - Tb/2; //Calcul de T2 + + TCNT1 = 0; // remise de TCNT1 (counter 16bits) + Gx = calculG(Tx,T2,T1x_cal); + Gy = calculG(Ty,T2,T1y_cal); + Timer1 = TCNT1; + //remise dans l'état de capture + etat = strt_at_Ta; } } diff --git a/n/accel/calcul.c b/n/accel/calcul.c new file mode 100644 index 0000000..f8e02bb --- /dev/null +++ b/n/accel/calcul.c @@ -0,0 +1,68 @@ +/* calcul.c */ +/* Accelerometre {{{ + * + * Copyright (C) 2004 Thomas Burg + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "calcul.h" +#include + +/* +AutoDec */ +/* -AutoDec */ + +uint16_t T2_cal; +uint16_t T1x_cal; +uint16_t T1y_cal; +uint32_t K; + +volatile enum T_etat etat; + +void +calibration(void) +{ + uint8_t i = 0; + uint32_t T1x = 0; + uint32_t T1y = 0; + uint32_t T2 = 0; + while (i < 8 ) + { + if( etat == calcul) + { + T1x += Tb; + T1y += Td - Tc; + T2 = (Td - (Td - Tc)/2) - Tb/2; + i++; + etat = strt_at_Ta; + } + } + T2_cal = T2 / 8; + T1x_cal = T1x / 8; + T1y_cal = T1y / 8; + K = ( 4 * ( T2_cal * BIT_SCALE_FACTOR) / T2_cal); +} + +int16_t +calculG ( uint16_t T1, uint16_t T2, uint16_t Zcal) +{ + uint32_t Zactual; + Zactual = Zcal * T2 / T2_cal; + return ( K * ( T1 - Zactual) / T2); +} diff --git a/n/accel/calcul.h b/n/accel/calcul.h new file mode 100644 index 0000000..744df17 --- /dev/null +++ b/n/accel/calcul.h @@ -0,0 +1,53 @@ +#ifndef calcul_h +#define calcul_h +/* calcul.h */ +/* Accelerometre {{{ + * + * Copyright (C) 2004 Thomas Burg + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/* +AutoDec */ +/* -AutoDec */ + +/* Variable globale, propre au calcul + */ +#include + +#define BIT_SCALE_FACTOR 60000 + +enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul }; + +extern uint16_t T2_cal; +extern uint16_t T1x_cal; +extern uint16_t T1y_cal; +extern uint32_t K; + +extern volatile uint16_t Tb,Td,Tc; + +extern volatile enum T_etat etat; + +/* Fonction + */ +void calibration(void); +int16_t calculG(uint16_t T1, uint16_t T2,uint16_t Zcal); + +#endif /* calcul_h */ -- cgit v1.2.3