camera/color-finder: add angle

diff --git a/src/camera/color-finder/control/CRecognition.cpp b/src/camera/color-finder/control/CRecognition.cpp
index 1bbb260b2324d7d20787934daac4ce461a3f92e2..c0234a2509dac58371b045305a2761b1d648fa88 100644 (file)
--- a/src/camera/color-finder/control/CRecognition.cpp
+++ b/src/camera/color-finder/control/CRecognition.cpp
@@ -1,9 +1,11 @@
 #include <CRecognition.h>
+#include <math.h>
 
 #define min(a,b) ((a) < (b) ? (a) : (b))
 #define max(a,b) ((a) > (b) ? (a) : (b))
 
 bool white = false;
+
 //Vymazani promennych a jejich nastaveni na pocatecni hodnoty
 CRecognition::CRecognition()
 {
@@ -222,7 +224,8 @@ SPixelPosition CRecognition::findSegment(CRawImage* image)
                result.x = segmentArray[index].x;
                result.y = segmentArray[index].y;
                result.info = true;
-               //fprintf(stdout,"xy = %i %i\n", result.x, result.y);
+               result.uhel = ((atan2(result.x - 320 , result.y - 240)-M_PI/2)*180)/ M_PI;
+               //fprintf(stdout,"xyu = %i %i %lf \n", result.x, result.y, result.uhel);
        } 
 
        //vykreslime vysledek

diff --git a/src/camera/color-finder/control/CRecognition.h b/src/camera/color-finder/control/CRecognition.h
index 7d858fe85f49518ce42c843f98c8abdf32b07669..c97c7410f26df97ff27aa38707850f3cd6433bc1 100644 (file)
--- a/src/camera/color-finder/control/CRecognition.h
+++ b/src/camera/color-finder/control/CRecognition.h
@@ -21,6 +21,7 @@ typedef struct{
        int x;
        int y;
         bool info;
+       double uhel;
 }SPixelPosition;
 
 class CRecognition

diff --git a/src/camera/color-finder/main/revue.cpp b/src/camera/color-finder/main/revue.cpp
index c16d58f38e7d444f39aa7bea7743c42d70f34776..ad6918d9efbc3e5da43e3b936d3e003bfbf148d3 100644 (file)
--- a/src/camera/color-finder/main/revue.cpp
+++ b/src/camera/color-finder/main/revue.cpp
@@ -111,9 +111,10 @@ void send_data(SPixelPosition pos)
         orte.camera_result.y = pos.y;
         orte.camera_result.target_valid = pos.info; // OK / NOT OK
         orte.camera_result.data_valid = data;
+       orte.camera_result.angle_deg = pos.uhel;
 
         ORTEPublicationSend(orte.publication_camera_result);
-        fprintf(stderr, "ORTE: x: %d y: %d, info: %d \n", pos.x, pos.y, pos.info);
+        fprintf(stderr, "ORTE: x: %d y: %d, info: %d uhel: %lf data_valid: %d \n", pos.x, pos.y, pos.info, pos.uhel, data);
 }
 
 /* Fce pro zapnuti a vypnuti rozpoznavani */

diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index eae458802c6d4f2f3f83d104a3317f1ae86ee3ab..8133653ed335af52d4be7d10d4eb11d49b2a43ac 100644 (file)
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -160,6 +160,7 @@ module camera {
        struct result {
                 octet x;
                 octet y;
+               double angle_deg;
 
                 boolean data_valid;
                 boolean target_valid;