Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

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?

UserRevisionLine numberNew 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 }