Robot_tennis / Mbed 2 deprecated Calc_laser

Cable de detection de laser

Dependencies:   mbed IHM

main.cpp@0:6d472204c7b4, 2022-06-06 (annotated)

Committer:
remiwan
Date:
Mon Jun 06 16:01:27 2022 +0000
Revision:
0:6d472204c7b4
qd;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
remiwan 0:6d472204c7b4 1 #include "IHM.h" // contient mbed.h
remiwan 0:6d472204c7b4 2 #include "mbed.h"
remiwan 0:6d472204c7b4 3 //#include <cmath>
remiwan 0:6d472204c7b4 4
remiwan 0:6d472204c7b4 5 IHM ihm; // définition de l'objet ihm (de classe IHM pour la NBOARD)
remiwan 0:6d472204c7b4 6 AnalogIn laser1(PC_2);
remiwan 0:6d472204c7b4 7 AnalogIn laser2(PC_0);
remiwan 0:6d472204c7b4 8 AnalogIn laser3(PC_1);
remiwan 0:6d472204c7b4 9 AnalogIn laser4(PA_4);
remiwan 0:6d472204c7b4 10 AnalogIn laser5(PA_3);
remiwan 0:6d472204c7b4 11 AnalogIn laser6(PC_4);
remiwan 0:6d472204c7b4 12 AnalogIn laser7(PC_5);
remiwan 0:6d472204c7b4 13 AnalogIn laser8(PB_1);
remiwan 0:6d472204c7b4 14
remiwan 0:6d472204c7b4 15 Serial pc(PC_1, PA_4);
remiwan 0:6d472204c7b4 16
remiwan 0:6d472204c7b4 17
remiwan 0:6d472204c7b4 18
remiwan 0:6d472204c7b4 19 void calculcoord(float Xballe, float Yballe, float dist);
remiwan 0:6d472204c7b4 20 void calcul_dist(float courant_laser);
remiwan 0:6d472204c7b4 21 void calcul_dist_double_laser();
remiwan 0:6d472204c7b4 22
remiwan 0:6d472204c7b4 23
remiwan 0:6d472204c7b4 24 float courant_laser[8] = {laser1, laser2, laser3, laser4, laser5, laser6, laser7, laser8};
remiwan 0:6d472204c7b4 25 float distance_laser_fct[8] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
remiwan 0:6d472204c7b4 26 float distance_laser;
remiwan 0:6d472204c7b4 27
remiwan 0:6d472204c7b4 28 bool balle_inaccessible = false;
remiwan 0:6d472204c7b4 29 bool flag_balle_here = false;
remiwan 0:6d472204c7b4 30
remiwan 0:6d472204c7b4 31 double Odo_x, Odo_y, Odo_theta;
remiwan 0:6d472204c7b4 32 float Xballe;
remiwan 0:6d472204c7b4 33 float Yballe;
remiwan 0:6d472204c7b4 34
remiwan 0:6d472204c7b4 35 void calculcoord(float dist)
remiwan 0:6d472204c7b4 36 {
remiwan 0:6d472204c7b4 37 float Xcapt, Ycapt, Tcapt, Rcapt;
remiwan 0:6d472204c7b4 38
remiwan 0:6d472204c7b4 39 Xcapt = Rcapt * cos(Odo_theta /*+ Tcapt */);
remiwan 0:6d472204c7b4 40 Ycapt = Rcapt * sin(Odo_theta /*+ Tcapt */);
remiwan 0:6d472204c7b4 41
remiwan 0:6d472204c7b4 42 Xballe = Odo_x + Xcapt + dist * cos(Odo_theta);
remiwan 0:6d472204c7b4 43 Yballe = Odo_y + Ycapt + dist * sin(Odo_theta);
remiwan 0:6d472204c7b4 44 }
remiwan 0:6d472204c7b4 45
remiwan 0:6d472204c7b4 46 void calcul_dist(float * courant_laser_fct)
remiwan 0:6d472204c7b4 47 {
remiwan 0:6d472204c7b4 48 //courant = 0.00290 * distance + 3.8;
remiwan 0:6d472204c7b4 49 for(int i;i<8;i++)
remiwan 0:6d472204c7b4 50 {
remiwan 0:6d472204c7b4 51 distance_laser_fct[i] = courant_laser_fct[i] / 0.00290;
remiwan 0:6d472204c7b4 52 calculcoord(distance_laser);
remiwan 0:6d472204c7b4 53
remiwan 0:6d472204c7b4 54 if (distance_laser_fct[i]!= 0)
remiwan 0:6d472204c7b4 55 {
remiwan 0:6d472204c7b4 56 if((Xballe < 4000)|| (Xballe > 0) || (Yballe < 4000) || (Yballe > 0)) //vérifie si la balle est dans le terrain
remiwan 0:6d472204c7b4 57 balle_inaccessible = false;
remiwan 0:6d472204c7b4 58
remiwan 0:6d472204c7b4 59 else
remiwan 0:6d472204c7b4 60 balle_inaccessible = true;
remiwan 0:6d472204c7b4 61 }
remiwan 0:6d472204c7b4 62 }
remiwan 0:6d472204c7b4 63 }
remiwan 0:6d472204c7b4 64
remiwan 0:6d472204c7b4 65 void calcul_dist_double_laser()
remiwan 0:6d472204c7b4 66 {
remiwan 0:6d472204c7b4 67 if((abs(courant_laser[0] - courant_laser[1]) > 0.1) || (abs(courant_laser[2] - courant_laser[3]) > 0.1) || (abs(courant_laser[4] - courant_laser[5]) > 0.1) || (abs(courant_laser[6] - courant_laser[7]) > 0.1))
remiwan 0:6d472204c7b4 68 {
remiwan 0:6d472204c7b4 69 flag_balle_here = true;
remiwan 0:6d472204c7b4 70 courant_laser[0] = courant_laser[0] +0.055; //offset
remiwan 0:6d472204c7b4 71 courant_laser[3] = courant_laser[0] +0.055;
remiwan 0:6d472204c7b4 72 courant_laser[4] = courant_laser[0] +0.055;
remiwan 0:6d472204c7b4 73 courant_laser[7] = courant_laser[0] +0.055;
remiwan 0:6d472204c7b4 74
remiwan 0:6d472204c7b4 75
remiwan 0:6d472204c7b4 76 if((abs(courant_laser[0] - courant_laser[1]) > 0.1) || (abs(courant_laser[2] - courant_laser[3]) > 0.1))
remiwan 0:6d472204c7b4 77 {
remiwan 0:6d472204c7b4 78 distance_laser = courant_laser[1]/0.00290; //avant
remiwan 0:6d472204c7b4 79 }
remiwan 0:6d472204c7b4 80 else
remiwan 0:6d472204c7b4 81 {
remiwan 0:6d472204c7b4 82 distance_laser = courant_laser[5]/0.00290; //arrière
remiwan 0:6d472204c7b4 83 }
remiwan 0:6d472204c7b4 84 }
remiwan 0:6d472204c7b4 85 }
remiwan 0:6d472204c7b4 86
remiwan 0:6d472204c7b4 87 int main(void)
remiwan 0:6d472204c7b4 88 {
remiwan 0:6d472204c7b4 89 while(1)
remiwan 0:6d472204c7b4 90 {
remiwan 0:6d472204c7b4 91 pc.printf("loul");
remiwan 0:6d472204c7b4 92 calcul_dist(courant_laser);
remiwan 0:6d472204c7b4 93 calcul_dist_double_laser();
remiwan 0:6d472204c7b4 94
remiwan 0:6d472204c7b4 95 pc.printf("distance laser \n %f:",distance_laser);
remiwan 0:6d472204c7b4 96 pc.printf("distance laser 3\n %f:",distance_laser_fct[4]);
remiwan 0:6d472204c7b4 97 pc.printf("distance laser 4\n f:",distance_laser_fct[5]);
remiwan 0:6d472204c7b4 98 }
remiwan 0:6d472204c7b4 99 }