Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
Diff: odometrie.cpp
- Revision:
- 2:3066e614372f
- Parent:
- 0:38dbe2988e77
- Child:
- 5:3638d7e7c5c1
diff -r 0d76bc4e1aea -r 3066e614372f odometrie.cpp --- a/odometrie.cpp Wed Apr 17 15:49:42 2019 +0000 +++ b/odometrie.cpp Wed May 08 21:19:10 2019 +0000 @@ -4,29 +4,21 @@ #include "reglages.h" #include "math_precalc.h" -long int x_actuel; -//long int y_actuel; -//double angle; // angle du robot +double x_actuel; +double y_actuel; +double angle; // angle du robot -//long int nbr_tick_D_prec; -//long int nbr_tick_G_prec; - -//long int position_D_prec; -//long int position_G_prec; - -long int nbr_tick_prec; -/*.................. LE COMMENTAIRE EST L'ODOMETRIE AVEC CODEUSE............*/ +long int nbr_tick_D_prec; +long int nbr_tick_G_prec; + void init_odometrie() { x_actuel=X_INIT; - //y_actuel=Y_INIT; - //angle=THETA_INIT; - //nbr_tick_D_prec=0; - //nbr_tick_G_prec=0; - //position_D_prec=0; - //position_G_prec=0; - nbr_tick_prec=0; + y_actuel=Y_INIT; + angle=THETA_INIT; + nbr_tick_D_prec=0; + nbr_tick_G_prec=0; } void actualise_position() @@ -36,32 +28,25 @@ */ /*------recuperation de la rotation des roues---------*/ - //long int nbr_tick_D=get_nbr_tick_D(); - //long int nbr_tick_G=get_nbr_tick_G(); - //long int position_D=-get_position_D(); - //long int position_G=get_position_G(); - long int nbr_tick = get_nbr_tick_G(); + long int nbr_tick_D=get_nbr_tick_D(); + long int nbr_tick_G=get_nbr_tick_G(); + //calcul du nombre de tick - //long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec; - //long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec; - //long int position_D_actuel=position_D-position_D_prec; - //long int position_G_actuel=position_G-position_G_prec; - long int nbr_tick_actuel=nbr_tick-nbr_tick_prec; + long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec; + long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec; + //sauvegarde - //nbr_tick_D_prec=nbr_tick_D; - //nbr_tick_G_prec=nbr_tick_G; - //position_D_prec=position_D; - //position_G_prec=position_G; - nbr_tick_prec=nbr_tick; - //double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues - //double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D; - //double dep_roue_G = position_G_actuel * DISTANCE_PAR_TICK_G; - //double dep_roue_D = position_D_actuel * DISTANCE_PAR_TICK_D; - double dep_roue = nbr_tick_actuel * DISTANCE_PAR_TICK_G; + nbr_tick_D_prec=nbr_tick_D; + nbr_tick_G_prec=nbr_tick_G; + + double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues + double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D; + + /*------calcul de la trajectoire---------*/ - x_actuel += (int) dep_roue+0.5; + // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle - /*if (dep_roue_G != dep_roue_D){ + if (dep_roue_G != dep_roue_D){ double R = 0; // rayon du cercle decrit par la trajectoire double d = 0; // vitesse du robot @@ -83,28 +68,31 @@ angle = borne_angle_r(angle); - x_actuel = (int) (cx + R * sin(angle) + 0.5); - y_actuel = (int) (cy - R * cos(angle) + 0.5); + x_actuel = cx + R * sin(angle); + y_actuel = cy - R * cos(angle); } else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite - x_actuel += (int) (dep_roue_G * cos(angle) + 0.5); - y_actuel += (int) (dep_roue_D * sin(angle) + 0.5); - }*/ - printf("tick : %d, x_actuel : %d\n", nbr_tick_actuel,x_actuel); - //printf("tick d : %d, tick g : %d, totaux D: %d, toaux G: %d x : %d, y : %d. angle : %lf\n", position_D_actuel,position_G_actuel,position_D,position_G, x_actuel, y_actuel, angle*180/PI); + x_actuel +=dep_roue_G * cos(angle); + y_actuel +=dep_roue_D * sin(angle); + } + + //printf("tick d : %d, tick g : %d, x : %lf, y : %lf. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI); + } -long int get_x_actuel() + + +double get_x_actuel() { return x_actuel; } -/*long int get_y_actuel() +double get_y_actuel() { return y_actuel; -}*/ +} -/*double get_angle() +double get_angle() { return angle*180/PI; -}*/ \ No newline at end of file +} \ No newline at end of file