summaryrefslogtreecommitdiff
path: root/n/accel
diff options
context:
space:
mode:
authorburg2004-12-13 01:00:55 +0000
committerburg2004-12-13 01:00:55 +0000
commitf4316ec9077a9271f6147f094c3a85b52168e77d (patch)
tree5e8c4b0615223bde4e975baf2af4911b471ccf30 /n/accel
parent82eca8372fdd8c56a136b09b534e4369ce4a2486 (diff)
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
Diffstat (limited to 'n/accel')
-rw-r--r--n/accel/accel.c152
-rw-r--r--n/accel/accel.h49
-rw-r--r--n/accel/calcul.c63
-rw-r--r--n/accel/calcul.h18
4 files changed, 194 insertions, 88 deletions
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 <avr/io.h>
-#include <inttypes.h>
-#include <avr/signal.h>
-#include <avr/interrupt.h>
-#include <string.h>
-#include <n/avr/rs232/rs232.h>
-#include <n/avr/proto/proto.h>
-#include <n/avr/utils/utils.h>
-
-#include "calcul.h"
-#include "avrconfig.h"
+#include "accel.h"
//#define DEBUG
@@ -53,9 +42,27 @@
* des donnes
* */
-//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; //dsactive les interruptions Input Capture
ACSR |= 0x04; //change d'entre : analogue comparator
TCCR1B |= _BV(ICES1); //trigge sur le front montant
- //TIMSK |= 0x20; // Ractive les interruptions Input Capture
break;
case rd_at_Tc :
debug_putc('C');
Tc = ICR1; // sauvegarde du registre Input Capture
- //TIMSK &= ~0x20; //dsactive les interruptions Input Capture
TCCR1B &= ~_BV(ICES1); // trigge sur le front descendant
- //TIMSK |= 0x20; // Ractive 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; //dsactive les interruptions Input Capture
+ etat = record;
ACSR &= ~0x04; //change d'entre : IPC
TCCR1B |= _BV(ICES1); //trigge sur le front montant
- //TIMSK |= 0x20; // Ractive 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'entre : 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 plutt
- 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 crass
+ 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 <avr/io.h>
+#include <inttypes.h>
+#include <avr/signal.h>
+#include <avr/interrupt.h>
+#include <string.h>
+#include <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+#include <n/avr/utils/utils.h>
+
+
+#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 <inttypes.h>
/* +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'entre : 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'entre : 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 <inttypes.h>
-#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 */