Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
odometrie.cpp@0:38dbe2988e77, 2019-04-17 (annotated)
- Committer:
- GuillaumeCH
- Date:
- Wed Apr 17 09:05:21 2019 +0000
- Revision:
- 0:38dbe2988e77
- Child:
- 2:3066e614372f
oui
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GuillaumeCH | 0:38dbe2988e77 | 1 | #include "mbed.h" |
GuillaumeCH | 0:38dbe2988e77 | 2 | #include "odometrie.h" |
GuillaumeCH | 0:38dbe2988e77 | 3 | #include "hardware.h" |
GuillaumeCH | 0:38dbe2988e77 | 4 | #include "reglages.h" |
GuillaumeCH | 0:38dbe2988e77 | 5 | #include "math_precalc.h" |
GuillaumeCH | 0:38dbe2988e77 | 6 | |
GuillaumeCH | 0:38dbe2988e77 | 7 | long int x_actuel; |
GuillaumeCH | 0:38dbe2988e77 | 8 | //long int y_actuel; |
GuillaumeCH | 0:38dbe2988e77 | 9 | //double angle; // angle du robot |
GuillaumeCH | 0:38dbe2988e77 | 10 | |
GuillaumeCH | 0:38dbe2988e77 | 11 | |
GuillaumeCH | 0:38dbe2988e77 | 12 | //long int nbr_tick_D_prec; |
GuillaumeCH | 0:38dbe2988e77 | 13 | //long int nbr_tick_G_prec; |
GuillaumeCH | 0:38dbe2988e77 | 14 | |
GuillaumeCH | 0:38dbe2988e77 | 15 | //long int position_D_prec; |
GuillaumeCH | 0:38dbe2988e77 | 16 | //long int position_G_prec; |
GuillaumeCH | 0:38dbe2988e77 | 17 | |
GuillaumeCH | 0:38dbe2988e77 | 18 | long int nbr_tick_prec; |
GuillaumeCH | 0:38dbe2988e77 | 19 | /*.................. LE COMMENTAIRE EST L'ODOMETRIE AVEC CODEUSE............*/ |
GuillaumeCH | 0:38dbe2988e77 | 20 | void init_odometrie() |
GuillaumeCH | 0:38dbe2988e77 | 21 | { |
GuillaumeCH | 0:38dbe2988e77 | 22 | x_actuel=X_INIT; |
GuillaumeCH | 0:38dbe2988e77 | 23 | //y_actuel=Y_INIT; |
GuillaumeCH | 0:38dbe2988e77 | 24 | //angle=THETA_INIT; |
GuillaumeCH | 0:38dbe2988e77 | 25 | //nbr_tick_D_prec=0; |
GuillaumeCH | 0:38dbe2988e77 | 26 | //nbr_tick_G_prec=0; |
GuillaumeCH | 0:38dbe2988e77 | 27 | //position_D_prec=0; |
GuillaumeCH | 0:38dbe2988e77 | 28 | //position_G_prec=0; |
GuillaumeCH | 0:38dbe2988e77 | 29 | nbr_tick_prec=0; |
GuillaumeCH | 0:38dbe2988e77 | 30 | } |
GuillaumeCH | 0:38dbe2988e77 | 31 | |
GuillaumeCH | 0:38dbe2988e77 | 32 | void actualise_position() |
GuillaumeCH | 0:38dbe2988e77 | 33 | { |
GuillaumeCH | 0:38dbe2988e77 | 34 | /* |
GuillaumeCH | 0:38dbe2988e77 | 35 | on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle |
GuillaumeCH | 0:38dbe2988e77 | 36 | */ |
GuillaumeCH | 0:38dbe2988e77 | 37 | |
GuillaumeCH | 0:38dbe2988e77 | 38 | /*------recuperation de la rotation des roues---------*/ |
GuillaumeCH | 0:38dbe2988e77 | 39 | //long int nbr_tick_D=get_nbr_tick_D(); |
GuillaumeCH | 0:38dbe2988e77 | 40 | //long int nbr_tick_G=get_nbr_tick_G(); |
GuillaumeCH | 0:38dbe2988e77 | 41 | //long int position_D=-get_position_D(); |
GuillaumeCH | 0:38dbe2988e77 | 42 | //long int position_G=get_position_G(); |
GuillaumeCH | 0:38dbe2988e77 | 43 | long int nbr_tick = get_nbr_tick_G(); |
GuillaumeCH | 0:38dbe2988e77 | 44 | //calcul du nombre de tick |
GuillaumeCH | 0:38dbe2988e77 | 45 | //long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec; |
GuillaumeCH | 0:38dbe2988e77 | 46 | //long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec; |
GuillaumeCH | 0:38dbe2988e77 | 47 | //long int position_D_actuel=position_D-position_D_prec; |
GuillaumeCH | 0:38dbe2988e77 | 48 | //long int position_G_actuel=position_G-position_G_prec; |
GuillaumeCH | 0:38dbe2988e77 | 49 | long int nbr_tick_actuel=nbr_tick-nbr_tick_prec; |
GuillaumeCH | 0:38dbe2988e77 | 50 | //sauvegarde |
GuillaumeCH | 0:38dbe2988e77 | 51 | //nbr_tick_D_prec=nbr_tick_D; |
GuillaumeCH | 0:38dbe2988e77 | 52 | //nbr_tick_G_prec=nbr_tick_G; |
GuillaumeCH | 0:38dbe2988e77 | 53 | //position_D_prec=position_D; |
GuillaumeCH | 0:38dbe2988e77 | 54 | //position_G_prec=position_G; |
GuillaumeCH | 0:38dbe2988e77 | 55 | nbr_tick_prec=nbr_tick; |
GuillaumeCH | 0:38dbe2988e77 | 56 | //double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues |
GuillaumeCH | 0:38dbe2988e77 | 57 | //double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D; |
GuillaumeCH | 0:38dbe2988e77 | 58 | //double dep_roue_G = position_G_actuel * DISTANCE_PAR_TICK_G; |
GuillaumeCH | 0:38dbe2988e77 | 59 | //double dep_roue_D = position_D_actuel * DISTANCE_PAR_TICK_D; |
GuillaumeCH | 0:38dbe2988e77 | 60 | double dep_roue = nbr_tick_actuel * DISTANCE_PAR_TICK_G; |
GuillaumeCH | 0:38dbe2988e77 | 61 | /*------calcul de la trajectoire---------*/ |
GuillaumeCH | 0:38dbe2988e77 | 62 | x_actuel += (int) dep_roue+0.5; |
GuillaumeCH | 0:38dbe2988e77 | 63 | // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle |
GuillaumeCH | 0:38dbe2988e77 | 64 | /*if (dep_roue_G != dep_roue_D){ |
GuillaumeCH | 0:38dbe2988e77 | 65 | |
GuillaumeCH | 0:38dbe2988e77 | 66 | double R = 0; // rayon du cercle decrit par la trajectoire |
GuillaumeCH | 0:38dbe2988e77 | 67 | double d = 0; // vitesse du robot |
GuillaumeCH | 0:38dbe2988e77 | 68 | double cx = 0; // position du centre du cercle decrit par la trajectoire |
GuillaumeCH | 0:38dbe2988e77 | 69 | double cy = 0; |
GuillaumeCH | 0:38dbe2988e77 | 70 | |
GuillaumeCH | 0:38dbe2988e77 | 71 | R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle |
GuillaumeCH | 0:38dbe2988e77 | 72 | cx = x_actuel - R * sin(angle); |
GuillaumeCH | 0:38dbe2988e77 | 73 | cy = y_actuel + R * cos(angle); |
GuillaumeCH | 0:38dbe2988e77 | 74 | d = (dep_roue_G + dep_roue_D) / 2; |
GuillaumeCH | 0:38dbe2988e77 | 75 | |
GuillaumeCH | 0:38dbe2988e77 | 76 | // mise à jour des coordonnées du robot |
GuillaumeCH | 0:38dbe2988e77 | 77 | if (dep_roue_G + dep_roue_D != 0){ |
GuillaumeCH | 0:38dbe2988e77 | 78 | angle += d / R; |
GuillaumeCH | 0:38dbe2988e77 | 79 | } |
GuillaumeCH | 0:38dbe2988e77 | 80 | else{ |
GuillaumeCH | 0:38dbe2988e77 | 81 | angle += dep_roue_D * 2 / ECART_ROUE; |
GuillaumeCH | 0:38dbe2988e77 | 82 | } |
GuillaumeCH | 0:38dbe2988e77 | 83 | |
GuillaumeCH | 0:38dbe2988e77 | 84 | angle = borne_angle_r(angle); |
GuillaumeCH | 0:38dbe2988e77 | 85 | |
GuillaumeCH | 0:38dbe2988e77 | 86 | x_actuel = (int) (cx + R * sin(angle) + 0.5); |
GuillaumeCH | 0:38dbe2988e77 | 87 | y_actuel = (int) (cy - R * cos(angle) + 0.5); |
GuillaumeCH | 0:38dbe2988e77 | 88 | } |
GuillaumeCH | 0:38dbe2988e77 | 89 | else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite |
GuillaumeCH | 0:38dbe2988e77 | 90 | x_actuel += (int) (dep_roue_G * cos(angle) + 0.5); |
GuillaumeCH | 0:38dbe2988e77 | 91 | y_actuel += (int) (dep_roue_D * sin(angle) + 0.5); |
GuillaumeCH | 0:38dbe2988e77 | 92 | }*/ |
GuillaumeCH | 0:38dbe2988e77 | 93 | printf("tick : %d, x_actuel : %d\n", nbr_tick_actuel,x_actuel); |
GuillaumeCH | 0:38dbe2988e77 | 94 | //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); |
GuillaumeCH | 0:38dbe2988e77 | 95 | } |
GuillaumeCH | 0:38dbe2988e77 | 96 | |
GuillaumeCH | 0:38dbe2988e77 | 97 | long int get_x_actuel() |
GuillaumeCH | 0:38dbe2988e77 | 98 | { |
GuillaumeCH | 0:38dbe2988e77 | 99 | return x_actuel; |
GuillaumeCH | 0:38dbe2988e77 | 100 | } |
GuillaumeCH | 0:38dbe2988e77 | 101 | |
GuillaumeCH | 0:38dbe2988e77 | 102 | /*long int get_y_actuel() |
GuillaumeCH | 0:38dbe2988e77 | 103 | { |
GuillaumeCH | 0:38dbe2988e77 | 104 | return y_actuel; |
GuillaumeCH | 0:38dbe2988e77 | 105 | }*/ |
GuillaumeCH | 0:38dbe2988e77 | 106 | |
GuillaumeCH | 0:38dbe2988e77 | 107 | /*double get_angle() |
GuillaumeCH | 0:38dbe2988e77 | 108 | { |
GuillaumeCH | 0:38dbe2988e77 | 109 | return angle*180/PI; |
GuillaumeCH | 0:38dbe2988e77 | 110 | }*/ |