summaryrefslogtreecommitdiff
path: root/n/accel
diff options
context:
space:
mode:
authorburg2004-07-22 22:05:48 +0000
committerburg2004-07-22 22:05:48 +0000
commitca20561f2ee33afcd43e7a0cb80b1361342f6df9 (patch)
tree72438fead58a141442df3957588a2e632a0a4c42 /n/accel
parentffe7e636c41e401f7d690d46e10a81cd702248b6 (diff)
Ajout de fichiers et modife des fichiers existant.
Mesure possible
Diffstat (limited to 'n/accel')
-rw-r--r--n/accel/Makefile2
-rw-r--r--n/accel/README1
-rw-r--r--n/accel/accel.c62
-rw-r--r--n/accel/calcul.c68
-rw-r--r--n/accel/calcul.h53
5 files changed, 166 insertions, 20 deletions
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 <avr/interrupt.h>
#include <string.h>
#include <n/avr/rs232/rs232.h>
-#include "avrconfig.h"
#include <n/avr/proto/proto.h>
-#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 <inttypes.h>
+
+/* +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 <inttypes.h>
+
+#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 */