From 2f9746bd5c5dd83b66dbd832d34c22ab4afde10a Mon Sep 17 00:00:00 2001 From: burg Date: Tue, 21 Sep 2004 16:41:08 +0000 Subject: Correction de la méthode de calcul de l'accélération. --- n/accel/accel.c | 16 +++++++++------- n/accel/calcul.c | 30 +++++++++++++++++------------- n/accel/calcul.h | 4 ++-- 3 files changed, 28 insertions(+), 22 deletions(-) (limited to 'n/accel') diff --git a/n/accel/accel.c b/n/accel/accel.c index eaa848a..fb63fc9 100644 --- a/n/accel/accel.c +++ b/n/accel/accel.c @@ -130,6 +130,8 @@ 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); + break; case 't' : proto_send2('T',Timer1 >> 8, Timer1); break; case 'g' : proto_send2('x',Gx >> 8, Gx); @@ -142,7 +144,7 @@ test_callback (uint8_t c, uint8_t argc, proto_arg_t argv[]) rs232_putc('V'); proto_send2('y',Vy >> 8,Vy); break; - case 'z' : reset; + case 'z' : reset(); default : proto_send1('E',2); //erreur 2 Command incorrect } } @@ -161,12 +163,12 @@ int main (void) { //initialisation - rs232_init(); + rs232_init(); proto_init(test_callback,rs232_putc); proto_send0('Z'); - //initialisation du timer prescaler 254 + //initialisation du timer prescaler 256 TCCR1B = _BV(ICES1) | _BV(CS12); // Input Capture Interrupt enable & Timer 1 overflow TIMSK = _BV(TICIE1) | _BV(TOIE1); @@ -189,10 +191,10 @@ main (void) T2 = (Td + Tc - Tb) /2; // remise de TCNT1 pour mesurer le temps TCNT1 = 0; - Gx = calculG(Tx,T2_cal,G0x); - Gy = calculG(Ty,T2_cal,G0y); - Vx += (Gx) >> 8; - Vy += (Gy) >> 8; + 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 diff --git a/n/accel/calcul.c b/n/accel/calcul.c index cbaed7b..aeff534 100644 --- a/n/accel/calcul.c +++ b/n/accel/calcul.c @@ -41,31 +41,27 @@ int16_t G0y; void calibration(void) { - uint8_t i = 0; + uint16_t i = 0; uint32_t T1x = 0; uint32_t T1y = 0; - uint32_t T2 = 0; - while (i < 128 ) + uint32_t T2c = 0; + while (i < 1024 ) { if( etat == calcul) { T1x += Tb; T1y += Td - Tc; //T2 += (Td - (Td - Tc)/2) - Tb/2; - T2 += (Td + Tc - Tb) /2; + T2c += (Td + Tc - Tb) /2; i++; etat = strt_at_Ta; } } - 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; + 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)); + Vx = 0; Vy = 0; } @@ -79,3 +75,11 @@ calculG ( uint16_t T1, uint16_t T2, int16_t Go) */ return (K * T1) / T2 - Go ; } +int16_t +calculGb(uint16_t T1, uint16_t T2, uint16_t T1cal) +{ + int32_t Zactual = T1cal * T2 / T2_cal; + int32_t Gtmp = K * (T1 - Zactual); + int16_t G = Gtmp / T2; + return G; +} diff --git a/n/accel/calcul.h b/n/accel/calcul.h index e474fa5..27d0698 100644 --- a/n/accel/calcul.h +++ b/n/accel/calcul.h @@ -32,7 +32,7 @@ */ #include -#define BIT_SCALE_FACTOR 60000 +#define BIT_SCALE_FACTOR 4096 enum T_etat {strt_at_Ta, rd_at_Tb, rd_at_Tc, rd_at_Td, calcul }; @@ -53,6 +53,6 @@ extern volatile enum T_etat etat; */ 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); #endif /* calcul_h */ -- cgit v1.2.3