Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
odometrie.cpp@2:3066e614372f, 2019-05-08 (annotated)
- Committer:
- GuillaumeCH
- Date:
- Wed May 08 21:19:10 2019 +0000
- Revision:
- 2:3066e614372f
- Parent:
- 0:38dbe2988e77
- Child:
- 5:3638d7e7c5c1
a
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 | 2:3066e614372f | 7 | double x_actuel; |
GuillaumeCH | 2:3066e614372f | 8 | double y_actuel; |
GuillaumeCH | 2:3066e614372f | 9 | double angle; // angle du robot |
GuillaumeCH | 0:38dbe2988e77 | 10 | |
GuillaumeCH | 0:38dbe2988e77 | 11 | |
GuillaumeCH | 2:3066e614372f | 12 | long int nbr_tick_D_prec; |
GuillaumeCH | 2:3066e614372f | 13 | long int nbr_tick_G_prec; |
GuillaumeCH | 2:3066e614372f | 14 | |
GuillaumeCH | 0:38dbe2988e77 | 15 | void init_odometrie() |
GuillaumeCH | 0:38dbe2988e77 | 16 | { |
GuillaumeCH | 0:38dbe2988e77 | 17 | x_actuel=X_INIT; |
GuillaumeCH | 2:3066e614372f | 18 | y_actuel=Y_INIT; |
GuillaumeCH | 2:3066e614372f | 19 | angle=THETA_INIT; |
GuillaumeCH | 2:3066e614372f | 20 | nbr_tick_D_prec=0; |
GuillaumeCH | 2:3066e614372f | 21 | nbr_tick_G_prec=0; |
GuillaumeCH | 0:38dbe2988e77 | 22 | } |
GuillaumeCH | 0:38dbe2988e77 | 23 | |
GuillaumeCH | 0:38dbe2988e77 | 24 | void actualise_position() |
GuillaumeCH | 0:38dbe2988e77 | 25 | { |
GuillaumeCH | 0:38dbe2988e77 | 26 | /* |
GuillaumeCH | 0:38dbe2988e77 | 27 | on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle |
GuillaumeCH | 0:38dbe2988e77 | 28 | */ |
GuillaumeCH | 0:38dbe2988e77 | 29 | |
GuillaumeCH | 0:38dbe2988e77 | 30 | /*------recuperation de la rotation des roues---------*/ |
GuillaumeCH | 2:3066e614372f | 31 | long int nbr_tick_D=get_nbr_tick_D(); |
GuillaumeCH | 2:3066e614372f | 32 | long int nbr_tick_G=get_nbr_tick_G(); |
GuillaumeCH | 2:3066e614372f | 33 | |
GuillaumeCH | 0:38dbe2988e77 | 34 | //calcul du nombre de tick |
GuillaumeCH | 2:3066e614372f | 35 | long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec; |
GuillaumeCH | 2:3066e614372f | 36 | long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec; |
GuillaumeCH | 2:3066e614372f | 37 | |
GuillaumeCH | 0:38dbe2988e77 | 38 | //sauvegarde |
GuillaumeCH | 2:3066e614372f | 39 | nbr_tick_D_prec=nbr_tick_D; |
GuillaumeCH | 2:3066e614372f | 40 | nbr_tick_G_prec=nbr_tick_G; |
GuillaumeCH | 2:3066e614372f | 41 | |
GuillaumeCH | 2:3066e614372f | 42 | double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues |
GuillaumeCH | 2:3066e614372f | 43 | double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D; |
GuillaumeCH | 2:3066e614372f | 44 | |
GuillaumeCH | 2:3066e614372f | 45 | |
GuillaumeCH | 0:38dbe2988e77 | 46 | /*------calcul de la trajectoire---------*/ |
GuillaumeCH | 2:3066e614372f | 47 | |
GuillaumeCH | 0:38dbe2988e77 | 48 | // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle |
GuillaumeCH | 2:3066e614372f | 49 | if (dep_roue_G != dep_roue_D){ |
GuillaumeCH | 0:38dbe2988e77 | 50 | |
GuillaumeCH | 0:38dbe2988e77 | 51 | double R = 0; // rayon du cercle decrit par la trajectoire |
GuillaumeCH | 0:38dbe2988e77 | 52 | double d = 0; // vitesse du robot |
GuillaumeCH | 0:38dbe2988e77 | 53 | double cx = 0; // position du centre du cercle decrit par la trajectoire |
GuillaumeCH | 0:38dbe2988e77 | 54 | double cy = 0; |
GuillaumeCH | 0:38dbe2988e77 | 55 | |
GuillaumeCH | 0:38dbe2988e77 | 56 | R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle |
GuillaumeCH | 0:38dbe2988e77 | 57 | cx = x_actuel - R * sin(angle); |
GuillaumeCH | 0:38dbe2988e77 | 58 | cy = y_actuel + R * cos(angle); |
GuillaumeCH | 0:38dbe2988e77 | 59 | d = (dep_roue_G + dep_roue_D) / 2; |
GuillaumeCH | 0:38dbe2988e77 | 60 | |
GuillaumeCH | 0:38dbe2988e77 | 61 | // mise à jour des coordonnées du robot |
GuillaumeCH | 0:38dbe2988e77 | 62 | if (dep_roue_G + dep_roue_D != 0){ |
GuillaumeCH | 0:38dbe2988e77 | 63 | angle += d / R; |
GuillaumeCH | 0:38dbe2988e77 | 64 | } |
GuillaumeCH | 0:38dbe2988e77 | 65 | else{ |
GuillaumeCH | 0:38dbe2988e77 | 66 | angle += dep_roue_D * 2 / ECART_ROUE; |
GuillaumeCH | 0:38dbe2988e77 | 67 | } |
GuillaumeCH | 0:38dbe2988e77 | 68 | |
GuillaumeCH | 0:38dbe2988e77 | 69 | angle = borne_angle_r(angle); |
GuillaumeCH | 0:38dbe2988e77 | 70 | |
GuillaumeCH | 2:3066e614372f | 71 | x_actuel = cx + R * sin(angle); |
GuillaumeCH | 2:3066e614372f | 72 | y_actuel = cy - R * cos(angle); |
GuillaumeCH | 0:38dbe2988e77 | 73 | } |
GuillaumeCH | 0:38dbe2988e77 | 74 | else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite |
GuillaumeCH | 2:3066e614372f | 75 | x_actuel +=dep_roue_G * cos(angle); |
GuillaumeCH | 2:3066e614372f | 76 | y_actuel +=dep_roue_D * sin(angle); |
GuillaumeCH | 2:3066e614372f | 77 | } |
GuillaumeCH | 2:3066e614372f | 78 | |
GuillaumeCH | 2:3066e614372f | 79 | //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); |
GuillaumeCH | 2:3066e614372f | 80 | |
GuillaumeCH | 0:38dbe2988e77 | 81 | } |
GuillaumeCH | 0:38dbe2988e77 | 82 | |
GuillaumeCH | 2:3066e614372f | 83 | |
GuillaumeCH | 2:3066e614372f | 84 | |
GuillaumeCH | 2:3066e614372f | 85 | double get_x_actuel() |
GuillaumeCH | 0:38dbe2988e77 | 86 | { |
GuillaumeCH | 0:38dbe2988e77 | 87 | return x_actuel; |
GuillaumeCH | 0:38dbe2988e77 | 88 | } |
GuillaumeCH | 0:38dbe2988e77 | 89 | |
GuillaumeCH | 2:3066e614372f | 90 | double get_y_actuel() |
GuillaumeCH | 0:38dbe2988e77 | 91 | { |
GuillaumeCH | 0:38dbe2988e77 | 92 | return y_actuel; |
GuillaumeCH | 2:3066e614372f | 93 | } |
GuillaumeCH | 0:38dbe2988e77 | 94 | |
GuillaumeCH | 2:3066e614372f | 95 | double get_angle() |
GuillaumeCH | 0:38dbe2988e77 | 96 | { |
GuillaumeCH | 0:38dbe2988e77 | 97 | return angle*180/PI; |
GuillaumeCH | 2:3066e614372f | 98 | } |