Corentin Courtot / Mbed 2 deprecated Asserve

asser

Dependencies:   mbed X_NUCLEO_IHM02A1

odometrie.cpp@3:1dba6eca01ad, 2019-05-06 (annotated)

Committer:
GuillaumeCH
Date:
Mon May 06 13:48:45 2019 +0000
Revision:
3:1dba6eca01ad
Parent:
2:5764f89a27f6
Child:
4:deef042e9c02
O

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Coconara 0:6ca63d45f0ee 1 #include "mbed.h"
Coconara 0:6ca63d45f0ee 2 #include "odometrie.h"
Coconara 0:6ca63d45f0ee 3 #include "hardware.h"
Coconara 0:6ca63d45f0ee 4 #include "reglages.h"
Coconara 0:6ca63d45f0ee 5 #include "math_precalc.h"
Coconara 0:6ca63d45f0ee 6
Coconara 0:6ca63d45f0ee 7 long int x_actuel;
Coconara 0:6ca63d45f0ee 8 long int y_actuel;
Coconara 0:6ca63d45f0ee 9 double angle; // angle du robot
Coconara 0:6ca63d45f0ee 10
Coconara 0:6ca63d45f0ee 11
Coconara 0:6ca63d45f0ee 12 long int nbr_tick_D_prec;
Coconara 0:6ca63d45f0ee 13 long int nbr_tick_G_prec;
Coconara 0:6ca63d45f0ee 14
Coconara 0:6ca63d45f0ee 15 void init_odometrie()
Coconara 0:6ca63d45f0ee 16 {
Coconara 0:6ca63d45f0ee 17 x_actuel=X_INIT;
Coconara 0:6ca63d45f0ee 18 y_actuel=Y_INIT;
GuillaumeCH 2:5764f89a27f6 19 angle=THETA_INIT;
Coconara 0:6ca63d45f0ee 20 nbr_tick_D_prec=0;
Coconara 0:6ca63d45f0ee 21 nbr_tick_G_prec=0;
Coconara 0:6ca63d45f0ee 22 }
Coconara 0:6ca63d45f0ee 23
Coconara 0:6ca63d45f0ee 24 void actualise_position()
Coconara 0:6ca63d45f0ee 25 {
Coconara 0:6ca63d45f0ee 26 /*
Coconara 0:6ca63d45f0ee 27 on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
Coconara 0:6ca63d45f0ee 28 */
Coconara 0:6ca63d45f0ee 29
Coconara 0:6ca63d45f0ee 30 /*------recuperation de la rotation des roues---------*/
Coconara 0:6ca63d45f0ee 31 long int nbr_tick_D=get_nbr_tick_D();
Coconara 0:6ca63d45f0ee 32 long int nbr_tick_G=get_nbr_tick_G();
Coconara 0:6ca63d45f0ee 33
Coconara 0:6ca63d45f0ee 34 //calcul du nombre de tick
Coconara 0:6ca63d45f0ee 35 long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec;
Coconara 0:6ca63d45f0ee 36 long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec;
Coconara 0:6ca63d45f0ee 37
Coconara 0:6ca63d45f0ee 38 //sauvegarde
Coconara 0:6ca63d45f0ee 39 nbr_tick_D_prec=nbr_tick_D;
Coconara 0:6ca63d45f0ee 40 nbr_tick_G_prec=nbr_tick_G;
Coconara 0:6ca63d45f0ee 41
Coconara 0:6ca63d45f0ee 42 double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues
Coconara 0:6ca63d45f0ee 43 double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D;
Coconara 0:6ca63d45f0ee 44
Coconara 0:6ca63d45f0ee 45
Coconara 0:6ca63d45f0ee 46 /*------calcul de la trajectoire---------*/
Coconara 0:6ca63d45f0ee 47
Coconara 0:6ca63d45f0ee 48 // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
Coconara 0:6ca63d45f0ee 49 if (dep_roue_G != dep_roue_D){
Coconara 0:6ca63d45f0ee 50
Coconara 0:6ca63d45f0ee 51 double R = 0; // rayon du cercle decrit par la trajectoire
Coconara 0:6ca63d45f0ee 52 double d = 0; // vitesse du robot
Coconara 0:6ca63d45f0ee 53 double cx = 0; // position du centre du cercle decrit par la trajectoire
Coconara 0:6ca63d45f0ee 54 double cy = 0;
Coconara 0:6ca63d45f0ee 55
Coconara 0:6ca63d45f0ee 56 R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle
Coconara 0:6ca63d45f0ee 57 cx = x_actuel - R * sin(angle);
Coconara 0:6ca63d45f0ee 58 cy = y_actuel + R * cos(angle);
Coconara 0:6ca63d45f0ee 59 d = (dep_roue_G + dep_roue_D) / 2;
Coconara 0:6ca63d45f0ee 60
Coconara 0:6ca63d45f0ee 61 // mise à jour des coordonnées du robot
Coconara 0:6ca63d45f0ee 62 if (dep_roue_G + dep_roue_D != 0){
Coconara 0:6ca63d45f0ee 63 angle += d / R;
Coconara 0:6ca63d45f0ee 64 }
Coconara 0:6ca63d45f0ee 65 else{
Coconara 0:6ca63d45f0ee 66 angle += dep_roue_D * 2 / ECART_ROUE;
Coconara 0:6ca63d45f0ee 67 }
Coconara 0:6ca63d45f0ee 68
Coconara 1:0690cf83f060 69 angle = borne_angle_r(angle);
Coconara 0:6ca63d45f0ee 70
Coconara 0:6ca63d45f0ee 71 x_actuel = (int) (cx + R * sin(angle) + 0.5);
Coconara 0:6ca63d45f0ee 72 y_actuel = (int) (cy - R * cos(angle) + 0.5);
Coconara 0:6ca63d45f0ee 73 }
Coconara 0:6ca63d45f0ee 74 else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite
Coconara 0:6ca63d45f0ee 75 x_actuel += (int) (dep_roue_G * cos(angle) + 0.5);
Coconara 0:6ca63d45f0ee 76 y_actuel += (int) (dep_roue_D * sin(angle) + 0.5);
Coconara 0:6ca63d45f0ee 77 }
Coconara 0:6ca63d45f0ee 78
Coconara 1:0690cf83f060 79 printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
Coconara 0:6ca63d45f0ee 80 }
Coconara 0:6ca63d45f0ee 81
GuillaumeCH 3:1dba6eca01ad 82 void actualise_position_test()
GuillaumeCH 3:1dba6eca01ad 83 {
GuillaumeCH 3:1dba6eca01ad 84
GuillaumeCH 3:1dba6eca01ad 85 //on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
GuillaumeCH 3:1dba6eca01ad 86
GuillaumeCH 3:1dba6eca01ad 87
GuillaumeCH 3:1dba6eca01ad 88 //------recuperation de la rotation des roues---------
GuillaumeCH 3:1dba6eca01ad 89 long int nbr_tick_D=get_nbr_tick_D();
GuillaumeCH 3:1dba6eca01ad 90 long int nbr_tick_G=get_nbr_tick_G();
GuillaumeCH 3:1dba6eca01ad 91
GuillaumeCH 3:1dba6eca01ad 92 //calcul du nombre de tick
GuillaumeCH 3:1dba6eca01ad 93 long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec;
GuillaumeCH 3:1dba6eca01ad 94 long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec;
GuillaumeCH 3:1dba6eca01ad 95
GuillaumeCH 3:1dba6eca01ad 96 //sauvegarde
GuillaumeCH 3:1dba6eca01ad 97 nbr_tick_D_prec=nbr_tick_D;
GuillaumeCH 3:1dba6eca01ad 98 nbr_tick_G_prec=nbr_tick_G;
GuillaumeCH 3:1dba6eca01ad 99
GuillaumeCH 3:1dba6eca01ad 100 double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues
GuillaumeCH 3:1dba6eca01ad 101 double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D;
GuillaumeCH 3:1dba6eca01ad 102
GuillaumeCH 3:1dba6eca01ad 103
GuillaumeCH 3:1dba6eca01ad 104 //------calcul de la trajectoire---------
GuillaumeCH 3:1dba6eca01ad 105
GuillaumeCH 3:1dba6eca01ad 106 // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
GuillaumeCH 3:1dba6eca01ad 107 if (dep_roue_G != dep_roue_D){
GuillaumeCH 3:1dba6eca01ad 108
GuillaumeCH 3:1dba6eca01ad 109 double R = 0; // rayon du cercle decrit par la trajectoire
GuillaumeCH 3:1dba6eca01ad 110 double d = 0; // vitesse du robot
GuillaumeCH 3:1dba6eca01ad 111 double cx = 0; // position du centre du cercle decrit par la trajectoire
GuillaumeCH 3:1dba6eca01ad 112 double cy = 0;
GuillaumeCH 3:1dba6eca01ad 113
GuillaumeCH 3:1dba6eca01ad 114 R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle
GuillaumeCH 3:1dba6eca01ad 115 cx = x_actuel - R * sin(angle);
GuillaumeCH 3:1dba6eca01ad 116 cy = y_actuel + R * cos(angle);
GuillaumeCH 3:1dba6eca01ad 117 d = (dep_roue_G + dep_roue_D) / 2;
GuillaumeCH 3:1dba6eca01ad 118
GuillaumeCH 3:1dba6eca01ad 119 // mise à jour des coordonnées du robot
GuillaumeCH 3:1dba6eca01ad 120 if (dep_roue_G + dep_roue_D != 0){
GuillaumeCH 3:1dba6eca01ad 121 angle += d / R;
GuillaumeCH 3:1dba6eca01ad 122 }
GuillaumeCH 3:1dba6eca01ad 123 else{
GuillaumeCH 3:1dba6eca01ad 124 angle += dep_roue_D * 2 / ECART_ROUE;
GuillaumeCH 3:1dba6eca01ad 125 }
GuillaumeCH 3:1dba6eca01ad 126
GuillaumeCH 3:1dba6eca01ad 127 angle = borne_angle_r(angle);
GuillaumeCH 3:1dba6eca01ad 128
GuillaumeCH 3:1dba6eca01ad 129 x_actuel = (int) (cx + R * sin(angle) + 0.5);
GuillaumeCH 3:1dba6eca01ad 130 y_actuel = (int) (cy - R * cos(angle) + 0.5);
GuillaumeCH 3:1dba6eca01ad 131 printf("cx : %lf cy : %lf ",cx + R * sin(angle) + 0.5,cy - R * cos(angle) + 0.5);
GuillaumeCH 3:1dba6eca01ad 132 }
GuillaumeCH 3:1dba6eca01ad 133 else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite
GuillaumeCH 3:1dba6eca01ad 134 x_actuel += (int) (dep_roue_G * cos(angle) + 0.5);
GuillaumeCH 3:1dba6eca01ad 135 y_actuel += (int) (dep_roue_D * sin(angle) + 0.5);
GuillaumeCH 3:1dba6eca01ad 136 }
GuillaumeCH 3:1dba6eca01ad 137
GuillaumeCH 3:1dba6eca01ad 138 printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
GuillaumeCH 3:1dba6eca01ad 139 }
GuillaumeCH 3:1dba6eca01ad 140
GuillaumeCH 3:1dba6eca01ad 141
Coconara 0:6ca63d45f0ee 142 long int get_x_actuel()
Coconara 0:6ca63d45f0ee 143 {
Coconara 0:6ca63d45f0ee 144 return x_actuel;
Coconara 0:6ca63d45f0ee 145 }
Coconara 0:6ca63d45f0ee 146
Coconara 0:6ca63d45f0ee 147 long int get_y_actuel()
Coconara 0:6ca63d45f0ee 148 {
Coconara 0:6ca63d45f0ee 149 return y_actuel;
Coconara 0:6ca63d45f0ee 150 }
Coconara 0:6ca63d45f0ee 151
Coconara 0:6ca63d45f0ee 152 double get_angle()
Coconara 0:6ca63d45f0ee 153 {
Coconara 1:0690cf83f060 154 return angle*180/PI;
Coconara 0:6ca63d45f0ee 155 }