Diff: main.cpp

Revision:

0:6d472204c7b4

diff -r 000000000000 -r 6d472204c7b4 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 06 16:01:27 2022 +0000
@@ -0,0 +1,99 @@
+#include "IHM.h"      // contient mbed.h
+#include "mbed.h"
+//#include <cmath>
+
+IHM ihm;  // définition de l'objet ihm (de classe IHM pour la NBOARD)
+AnalogIn laser1(PC_2); 
+AnalogIn laser2(PC_0);
+AnalogIn laser3(PC_1);
+AnalogIn laser4(PA_4);
+AnalogIn laser5(PA_3);
+AnalogIn laser6(PC_4);
+AnalogIn laser7(PC_5);
+AnalogIn laser8(PB_1);
+
+Serial pc(PC_1, PA_4);
+
+
+
+void calculcoord(float Xballe, float Yballe, float dist);
+void calcul_dist(float courant_laser);
+void calcul_dist_double_laser();
+
+
+float courant_laser[8] = {laser1, laser2, laser3, laser4, laser5, laser6, laser7, laser8};
+float distance_laser_fct[8] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+float distance_laser;
+
+bool balle_inaccessible = false;
+bool flag_balle_here = false;
+
+double Odo_x, Odo_y, Odo_theta;
+float Xballe;
+float Yballe;
+
+void calculcoord(float dist)
+{
+    float Xcapt, Ycapt, Tcapt, Rcapt;
+    
+    Xcapt = Rcapt * cos(Odo_theta /*+ Tcapt */);
+    Ycapt = Rcapt * sin(Odo_theta /*+ Tcapt */);
+    
+    Xballe = Odo_x + Xcapt + dist * cos(Odo_theta);
+    Yballe = Odo_y + Ycapt + dist * sin(Odo_theta);
+}
+
+void calcul_dist(float * courant_laser_fct)
+{
+    //courant = 0.00290 * distance + 3.8;
+    for(int i;i<8;i++)
+        {
+            distance_laser_fct[i] = courant_laser_fct[i] / 0.00290;
+            calculcoord(distance_laser);
+            
+            if (distance_laser_fct[i]!= 0)
+            {
+            if((Xballe < 4000)|| (Xballe > 0) || (Yballe < 4000) || (Yballe > 0)) //vérifie si la balle est dans le terrain
+                balle_inaccessible = false;
+            
+            else
+                balle_inaccessible = true;
+            }
+        }
+}
+
+void calcul_dist_double_laser()
+{
+    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))
+        {
+            flag_balle_here = true;
+            courant_laser[0] = courant_laser[0] +0.055; //offset 
+            courant_laser[3] = courant_laser[0] +0.055; 
+            courant_laser[4] = courant_laser[0] +0.055;
+            courant_laser[7] = courant_laser[0] +0.055;
+
+            
+            if((abs(courant_laser[0] - courant_laser[1]) > 0.1) || (abs(courant_laser[2] - courant_laser[3]) > 0.1))
+            {
+                distance_laser = courant_laser[1]/0.00290; //avant
+            }
+            else
+            {
+                distance_laser = courant_laser[5]/0.00290; //arrière
+            }       
+        }
+}
+
+int main(void)
+{
+    while(1)
+    {
+     pc.printf("loul");
+     calcul_dist(courant_laser);
+     calcul_dist_double_laser();
+     
+    pc.printf("distance laser \n %f:",distance_laser);
+    pc.printf("distance laser 3\n %f:",distance_laser_fct[4]);
+    pc.printf("distance laser 4\n f:",distance_laser_fct[5]);
+    }  
+}
\ No newline at end of file