From 44daa40214681aaca1e5b28ab4ce8d4567cbe984 Mon Sep 17 00:00:00 2001 From: burg Date: Fri, 23 Jul 2004 23:15:31 +0000 Subject: Correction des algos. Debut d'implémentation de la vitesse. --- n/accel/accel.c | 34 ++++++++++++++++++++++------------ n/accel/calcul.c | 31 ++++++++++++++++++++++--------- n/accel/calcul.h | 7 ++++++- 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 */ -- cgit v1.2.3