Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

odometrie.cpp@6:e1585b8bd07d, 2019-05-24 (annotated)

Committer:
JimmyAREM
Date:
Fri May 24 21:45:17 2019 +0000
Revision:
6:e1585b8bd07d
Parent:
5:3638d7e7c5c1 
arc

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
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 6:e1585b8bd07d 106 X_init = 22500.0; //22500
JimmyAREM 6:e1585b8bd07d 107 Y_init = 43500.0; // 45000
JimmyAREM 5:3638d7e7c5c1 108 //ANGLE_init = 0.0;
JimmyAREM 6:e1585b8bd07d 109 ANGLE_init = -90.0*PI/180.0;
JimmyAREM 5:3638d7e7c5c1 110 }
JimmyAREM 5:3638d7e7c5c1 111
JimmyAREM 5:3638d7e7c5c1 112
JimmyAREM 5:3638d7e7c5c1 113 void setEmplacementDepartJaune()
JimmyAREM 5:3638d7e7c5c1 114 {
JimmyAREM 6:e1585b8bd07d 115 X_init = 277500;
JimmyAREM 6:e1585b8bd07d 116 Y_init = 43500;
JimmyAREM 6:e1585b8bd07d 117 ANGLE_init = -90.0*PI/180.0;
JimmyAREM 6:e1585b8bd07d 118 }
JimmyAREM 6:e1585b8bd07d 119
JimmyAREM 6:e1585b8bd07d 120 void setEmplacementTest()
JimmyAREM 6:e1585b8bd07d 121 {
JimmyAREM 6:e1585b8bd07d 122 X_init = 30000;
JimmyAREM 6:e1585b8bd07d 123 Y_init = 30000;
JimmyAREM 6:e1585b8bd07d 124 ANGLE_init = 0.0;
GuillaumeCH 2:3066e614372f 125 }