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