Revision 1:a1a7c556e96a, committed 2018-05-11

Comitter:

Appalco

Date:

Fri May 11 13:35:55 2018 +0000

Parent:

0:497d7f448d3e

Commit message:

first sketch; found beacon but too many of them

Changed in this revision

--- a/LIDAR.cpp	Fri May 11 12:38:21 2018 +0000
+++ b/LIDAR.cpp	Fri May 11 13:35:55 2018 +0000
@@ -86,10 +86,37 @@
  * It should be called periodically by a low-priority background task.
  */
 void LIDAR::lookForBeacon() {
-    
-    distanceOfBeacon = 0;
-    angleOfBeacon = 0;
     
+    distanceOfBeacon = 0;
+    angleOfBeacon = 0;
+    short beacon_counter;
+    //copy measurement data
+    for (unsigned short i = 0; i < 360; i++)
+    {
+        distances_copy[i] = distances[i];
+    }
+    // search for gap more than 500mm
+    for(int i=1;i<360;i++)
+    {
+        // look for gap of 500mm and distance between 500-2000mm
+        if (abs(distances[i]-distances[i-1])>=500 && distances[i]>=500 && distances[i]<=2000)
+        {
+            printf("Debug: Found Gap at %d \n \r",i);
+            
+            // found edge look now for beacon
+            for(short u=1;u<9;u++) {
+                if(abs(distances[i+u]-distances[i+u-1])<=80 && abs(distances[i+u]-distances[i+u-1])>=60) {
+                    beacon_counter++;
+                    printf("Debug: Possible beacon atp at %d \n \r",i+u);
+                }
+                if(beacon_counter>=2) {
+                    printf("Debug: Found Beagon at %d and Distance %d \n \r",i+u,distances[i]); 
+                    distanceOfBeacon= distances[i];
+                    angleOfBeacon= i;
+                }
+            }
+        }
+    }
     // bitte implementieren!
 }
 

--- a/LIDAR.h	Fri May 11 12:38:21 2018 +0000
+++ b/LIDAR.h	Fri May 11 13:35:55 2018 +0000
@@ -49,6 +49,7 @@
         char        dataCounter;
         char        data[DATA_SIZE];
         short       distances[360];     // measured distance for every angle value, given in [mm]
+        short       distances_copy[360];     // measured distance for every angle value, given in [mm]
         short       distanceOfBeacon;   // distance of detected beacon, given in [mm]
         short       angleOfBeacon;      // angle of detected beacon, given in [degrees]
         

--- a/Main.cpp	Fri May 11 12:38:21 2018 +0000
+++ b/Main.cpp	Fri May 11 13:35:55 2018 +0000
@@ -88,7 +88,7 @@
     reset = 1; wait(0.1f);
     
     SerialServer serialServer(serial, lidar, controller);
-    
+    printf("Started\n");
     while (true) {
         
         led = !led; // toggle led