From f4316ec9077a9271f6147f094c3a85b52168e77d Mon Sep 17 00:00:00 2001 From: burg Date: Mon, 13 Dec 2004 01:00:55 +0000 Subject: Profonde modification, la plus part des fichiers a été réecrite. *Pb à la calibration *Pb sur le calule de l'accéleration => immoble = acceleration constante --- n/accel/accel.c | 152 ++++++++++++++++++++++++++++++++++++------------------- n/accel/accel.h | 49 ++++++++++++++++++ n/accel/calcul.c | 63 +++++++++++++---------- n/accel/calcul.h | 18 ++++--- 4 files changed, 194 insertions(+), 88 deletions(-) create mode 100644 n/accel/accel.h diff --git a/n/accel/accel.c b/n/accel/accel.c index ae0f022..875ed82 100644 --- a/n/accel/accel.c +++ b/n/accel/accel.c @@ -28,18 +28,7 @@ * */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "calcul.h" -#include "avrconfig.h" +#include "accel.h" //#define DEBUG @@ -53,9 +42,27 @@ * des données * */ -//enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul }; + + +volatile enum T_etat etat; + volatile uint16_t Tb,Tc,Td; -uint16_t Tx,Ty,T2; +volatile uint8_t calcul_rdy; +/* + * calcul_rdy = 0 Un calcul a été fait, on peut écraser Tx et Ty + * calcul_rdy = 1 Le calcul n'est pas encore terminer interdiction + * d'écraser Tx et Ty + */ +volatile uint8_t mode; +/* mode = 0 Mode normal; + * Mode = 1 Mode Mesure de T2 sur la voix x + * mode = 2 Mode Mesure de T2 sur la voix y + */ + +volatile uint16_t Tx,Ty,T2; +volatile uint16_t T2x=0; +volatile uint16_t T2y=0; + int16_t Gx,Gy; uint16_t Timer1; int16_t Vx,Vy; @@ -69,48 +76,81 @@ uint8_t command; SIGNAL(SIG_INPUT_CAPTURE1) { - //code de l'interruption + //code de l'interruption switch (etat) { case strt_at_Ta : debug_putc('A'); TCNT1 = 0; // remise de TCNT1 (counter 16bits) - TCCR1B &= ~_BV(ICES1); // trigge sur le front descendant - //TIMSK |= 0x20; //autorise les interruptions Input Capture - etat = rd_at_Tb; + if (mode !=0) + { + //va vers le mode config + if(mode ==1 ) + { + etat = rd_at_T2_x; + } + else + { + etat = rd_at_T2_y; + } + } + else + { + //v vers le mode normal + etat = rd_at_Tb; + TCCR1B &= ~_BV(ICES1); // trigge sur le front descendant + } break; case rd_at_Tb : debug_putc('B'); Tb = ICR1; // sauvegarde du registre Input Capture etat = rd_at_Tc; - //TIMSK &= ~0x20; //désactive les interruptions Input Capture ACSR |= 0x04; //change d'entrée : analogue comparator TCCR1B |= _BV(ICES1); //trigge sur le front montant - //TIMSK |= 0x20; // Réactive les interruptions Input Capture break; case rd_at_Tc : 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 - //TIMSK |= 0x20; // Réactive les interruptions Input Capture etat = rd_at_Td; break; case rd_at_Td : debug_putc('D'); Td = ICR1; // sauvegarde du registre Input Capture - etat = calcul; - //TIMSK &= ~0x20; //désactive les interruptions Input Capture + etat = record; ACSR &= ~0x04; //change d'entrée : IPC TCCR1B |= _BV(ICES1); //trigge sur le front montant - //TIMSK |= 0x20; // Réactive les interruptions Input Capture break; - case calcul : - // calcul trop long // - proto_send1('E',6); + case record : + if (calcul_rdy == 0 ) + { + Tx = Tb; // Calcul des Ton + Ty = Td - Tc; + // Todo T2 ne doit pas être calculer tout le temps + //T2 = (Td + Tc - Tb) /2; + calcul_rdy = 1; // Autorise le calcul + } + else + proto_send1('E',6);// calcul trop long // + etat = strt_at_Ta; + break; + case rd_at_T2_x : + T2x = ICR1; // sauvegard du registre input capture + etat = record_T2_x; + break; + case record_T2_x: + etat = strt_at_Ta; + break; + case rd_at_T2_y : + T2y = ICR1; // sauvegard du registre input capture + ACSR &= ~0x04; //change d'entrée : IPC + etat = record_T2_y; + break; + case record_T2_y: + etat = strt_at_Ta; break; default :proto_send1('E',3); - break; // Ce cas ne doit pas arriver + break; // Ce cas ne doit pas arriver } } @@ -121,6 +161,8 @@ SIGNAL(SIG_OVERFLOW1) void test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) +// fonction de gestion des communications +// {{{ { //proto_send (c, argc, argv); if (argc == 0) @@ -134,7 +176,9 @@ test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) break; case 'y' : proto_send2('Y',Ty >> 8, Tx); break; - case 'r' : proto_send2('R',T2 >> 8, T2); + case 'r' : proto_send2('z',T2x >> 8,T2x); + proto_send2('e',T2y >> 8, T2y); + proto_send2('R',T2 >> 8, T2); break; case 't' : proto_send2('T',Timer1 >> 8, Timer1); break; @@ -162,6 +206,7 @@ test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) else proto_send1('E',5); //Trop d'argument } } +//}}} int main (void) @@ -176,33 +221,34 @@ main (void) TCCR1B = _BV(ICES1) | _BV(CS12); // Input Capture Interrupt enable & Timer 1 overflow TIMSK = _BV(TICIE1) | _BV(TOIE1); + TCCR1B |= _BV(ICES1); //trigge sur le front montant + etat = strt_at_Ta; sei (); //Boucle while(1) { - if ( rs232_poll() ) - { - /* polling sur la reception serie - * */ - proto_accept(rs232_getc()); - } - // calcul des Tons - if (etat == calcul) - { - Tx = Tb; // Calcul des Ton - Ty = Td - Tc; - // Todo T2 ne doit pas être calculer tout le temps - T2 = (Td + Tc - Tb) /2; - // remise de TCNT1 pour mesurer le temps - TCNT1 = 0; - Gx = calculGb(Tx,T2,T1x_cal); - Gy = calculGb(Ty,T2,T1y_cal); - Vx +=Gx; - Vy +=Gy; - Timer1 = TCNT1; - //remise dans l'état de capture - //Todo : remettre dans cet état plutôt - etat = strt_at_Ta; + //proto_send0('b'); + if ( rs232_poll() ) + { + /* polling sur la reception serie + * */ + proto_accept(rs232_getc()); + } + // calcul si l'on peut + if (calcul_rdy == 1) + { + //proto_send0('c'); + // remise de TCNT1 pour mesurer le temps + TCNT1 = 0; + Gx = calculG(Tx,T1x_cal); + Gy = calculG(Ty,T1y_cal); + + // Tx et Ty ont été utilisé, on peut les écrasés + calcul_rdy = 0 ; + + Vx +=Gx; + Vy +=Gy; + Timer1 = TCNT1; if (command !=0) { diff --git a/n/accel/accel.h b/n/accel/accel.h new file mode 100644 index 0000000..9fca925 --- /dev/null +++ b/n/accel/accel.h @@ -0,0 +1,49 @@ +#ifndef accel_h +#define accel_h +/* accel.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 */ + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "calcul.h" +#include "avrconfig.h" + +enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td,record, rd_at_T2_x, + record_T2_x,rd_at_T2_y, record_T2_y}; + + + +#endif /* accel_h */ diff --git a/n/accel/calcul.c b/n/accel/calcul.c index aeff534..fe18b40 100644 --- a/n/accel/calcul.c +++ b/n/accel/calcul.c @@ -23,7 +23,6 @@ * * }}} */ #include "calcul.h" -#include /* +AutoDec */ /* -AutoDec */ @@ -33,47 +32,57 @@ uint16_t T1x_cal; uint16_t T1y_cal; uint32_t K; -volatile enum T_etat etat; - -int16_t G0x; -int16_t G0y; - void calibration(void) { - uint16_t i = 0; - uint32_t T1x = 0; - uint32_t T1y = 0; + mode =1; + uint32_t i = 0; uint32_t T2c = 0; - while (i < 1024 ) + while (i < 32768) + { + if( etat == record_T2_x) + { + T2c += T2x; + i++; + } + } + T2x = T2c / 32768; + i=0; + T2c = 0; + ACSR |= 0x04; //change d'entrée : analogue comparator + mode=2; + while (i < 32768) { - if( etat == calcul) + if( etat == record_T2_y) { - T1x += Tb; - T1y += Td - Tc; - //T2 += (Td - (Td - Tc)/2) - Tb/2; - T2c += (Td + Tc - Tb) /2; + T2c += T2y; i++; - etat = strt_at_Ta; } } - T2_cal = T2c / 1024; - T1x_cal = T1x / 1024; - T1y_cal = T1y / 1024; - K = (uint16_t)((uint32_t)( 4 * ( T2_cal * BIT_SCALE_FACTOR) / T2_cal)); + ACSR &= ~0x04; //change d'entrée : IPC + T2y = T2c / 32768; + T2 = (T2x+T2y)/2; + mode = 0; + uint8_t j = 0; + while (j==0) + { + if(calcul_rdy == 1) + { + T1x_cal = Tx; + T1y_cal = Ty; + j++; + } + } + K = BIT_SCALE_FACTOR * 8 / T2; Vx = 0; Vy = 0; - } int16_t -calculG ( uint16_t T1, uint16_t T2, int16_t Go) +calculG ( uint16_t T1, uint16_t T1_cal) { - /*uint32_t Zactual; - Zactual = Zcal * T2 / T2_cal; - return ( (int16_t) K * (int16_t)( T1 - Zactual) / (int16_t)T2); -*/ - return (K * T1) / T2 - Go ; + + return (T1 - T1_cal) * K ; } int16_t calculGb(uint16_t T1, uint16_t T2, uint16_t T1cal) diff --git a/n/accel/calcul.h b/n/accel/calcul.h index 27d0698..0d69059 100644 --- a/n/accel/calcul.h +++ b/n/accel/calcul.h @@ -30,29 +30,31 @@ /* Variable globale, propre au calcul */ +#include "accel.h" #include -#define BIT_SCALE_FACTOR 4096 - -enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul }; +#define BIT_SCALE_FACTOR 256 extern uint16_t T2_cal; extern uint16_t T1x_cal; extern uint16_t T1y_cal; +extern volatile uint8_t calcul_rdy; extern uint32_t K; -extern int16_t G0x; -extern int16_t G0y; extern int16_t Vx; extern int16_t Vy; -extern volatile uint16_t Tb,Td,Tc; +extern volatile uint16_t Tb,Td,Tc,T2; +extern volatile uint16_t Tx; +extern volatile uint16_t Ty; +extern volatile uint16_t T2x; +extern volatile uint16_t T2y; extern volatile enum T_etat etat; +extern volatile uint8_t mode; /* Fonction */ void calibration(void); -int16_t calculG(uint16_t T1, uint16_t T2,int16_t Zcal); -int16_t calculGb(uint16_t T1, uint16_t T2,uint16_t T1cal); +int16_t calculG(uint16_t T1, uint16_t T1_cal); #endif /* calcul_h */ -- cgit v1.2.3