Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

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?

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 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 }*/