summaryrefslogtreecommitdiff
path: root/n/accel
diff options
context:
space:
mode:
authorburg2004-07-23 23:15:31 +0000
committerburg2004-07-23 23:15:31 +0000
commit44daa40214681aaca1e5b28ab4ce8d4567cbe984 (patch)
tree5fb39666b10c4350a9897ac3579d8d2a54d4237b /n/accel
parentba4185353ca77fb691ef6fb6c99a9d452eda2475 (diff)
Correction des algos.
Debut d'implémentation de la vitesse.
Diffstat (limited to 'n/accel')
-rw-r--r--n/accel/accel.c34
-rw-r--r--n/accel/calcul.c31
-rw-r--r--n/accel/calcul.h7
3 files changed, 50 insertions, 22 deletions
diff --git a/n/accel/accel.c b/n/accel/accel.c
index e9eab90..0ec6552 100644
--- a/n/accel/accel.c
+++ b/n/accel/accel.c
@@ -57,6 +57,8 @@ volatile uint16_t Tb,Tc,Td;
uint16_t Tx,Ty,T2;
int16_t Gx,Gy;
uint16_t Timer1;
+int16_t Vx,Vy;
+int16_t Dx,Dy;
/* Variable Globale
*/
@@ -119,18 +121,25 @@ test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[])
//proto_send (c, argc, argv);
switch (c)
{
- case 'C' : proto_send0('C');
+ case 'c' : proto_send0('C');
calibration();
break;
- case 'X' : proto_send2('X',Tx >> 8, Tx);
+ case 'x' : proto_send2('X',Tx >> 8, Tx);
break;
- case 'Y' : proto_send2('Y',Ty >> 8, Tx);
+ case 'y' : proto_send2('Y',Ty >> 8, Tx);
break;
- case 'T' : proto_send2('T',Timer1 >> 8, Timer1);
+ case 't' : proto_send2('T',Timer1 >> 8, Timer1);
break;
- case 'G' : proto_send2('x',Gx >> 8, Gx);
+ case 'g' : proto_send2('x',Gx >> 8, Gx);
proto_send2('y',Gy >> 8, Gy);
break;
+ case 'k' : proto_send2('K',T2 >> 8, T2);
+ break;
+ case 'v' : rs232_putc('V');
+ proto_send2('x',Vx >> 8,Vx);
+ rs232_putc('V');
+ proto_send2('y',Vy >> 8,Vy);
+ break;
default : proto_send1('E',2); //erreur 2 Command incorrect
}
@@ -145,8 +154,8 @@ main (void)
proto_send0('Z');
- //initialisation du timer
- TCCR1B = _BV(ICES1) | _BV(CS10);
+ //initialisation du timer prescaler 254
+ TCCR1B = _BV(ICES1) | _BV(CS12);
// Input Capture Interrupt enable & Timer 1 overflow
TIMSK = _BV(TICIE1) | _BV(TOIE1);
sei ();
@@ -160,15 +169,16 @@ main (void)
// 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 - (Ty/2)) - Tb/2; //Calcul de T2
-
+ //T2 = (Td - (Ty/2)) - Tb/2; //Calcul de T2
+ T2 = (Td + Tc - Tb) /2;
TCNT1 = 0; // remise de TCNT1 (counter 16bits)
- Gx = calculG(Tx,T2,T1x_cal);
- Gy = calculG(Ty,T2,T1y_cal);
+ Gx = calculG(Tx,T2_cal,G0x);
+ Gy = calculG(Ty,T2_cal,G0y);
+ Vx += (Gx) >> 8;
+ Vy += (Gy) >> 8;
Timer1 = TCNT1;
//remise dans l'état de capture
etat = strt_at_Ta;
diff --git a/n/accel/calcul.c b/n/accel/calcul.c
index f8e02bb..cbaed7b 100644
--- a/n/accel/calcul.c
+++ b/n/accel/calcul.c
@@ -35,6 +35,9 @@ uint32_t K;
volatile enum T_etat etat;
+int16_t G0x;
+int16_t G0y;
+
void
calibration(void)
{
@@ -42,27 +45,37 @@ calibration(void)
uint32_t T1x = 0;
uint32_t T1y = 0;
uint32_t T2 = 0;
- while (i < 8 )
+ while (i < 128 )
{
if( etat == calcul)
{
T1x += Tb;
T1y += Td - Tc;
- T2 = (Td - (Td - Tc)/2) - Tb/2;
+ //T2 += (Td - (Td - Tc)/2) - Tb/2;
+ T2 += (Td + Tc - 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);
+ T2_cal = T2 / 128;
+ T1x_cal = T1x / 128;
+ T1y_cal = T1y / 128;
+ //K = ( 4 * ( T2_cal * BIT_SCALE_FACTOR) / T2_cal);
+ //K = 65336;
+ K = 8192;
+ G0x = K * T1x_cal / T2_cal;
+ G0y = K * T1y_cal / T2_cal;
+ Vx =0;
+ Vy = 0;
+
}
int16_t
-calculG ( uint16_t T1, uint16_t T2, uint16_t Zcal)
+calculG ( uint16_t T1, uint16_t T2, int16_t Go)
{
- uint32_t Zactual;
+ /*uint32_t Zactual;
Zactual = Zcal * T2 / T2_cal;
- return ( K * ( T1 - Zactual) / T2);
+ return ( (int16_t) K * (int16_t)( T1 - Zactual) / (int16_t)T2);
+*/
+ return (K * T1) / T2 - Go ;
}
diff --git a/n/accel/calcul.h b/n/accel/calcul.h
index 744df17..e474fa5 100644
--- a/n/accel/calcul.h
+++ b/n/accel/calcul.h
@@ -40,6 +40,10 @@ extern uint16_t T2_cal;
extern uint16_t T1x_cal;
extern uint16_t T1y_cal;
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;
@@ -48,6 +52,7 @@ extern volatile enum T_etat etat;
/* Fonction
*/
void calibration(void);
-int16_t calculG(uint16_t T1, uint16_t T2,uint16_t Zcal);
+int16_t calculG(uint16_t T1, uint16_t T2,int16_t Zcal);
+
#endif /* calcul_h */