robofsm/actuators: Send requested target color with control command to camera.

diff --git a/src/robofsm/actuators.c b/src/robofsm/actuators.c
index 33ac0b1150146ebf64fbd042057eb270110ef96c..b0c33a86d1b4330fb6fa06e744b2b320ebad37ef 100644 (file)
--- a/src/robofsm/actuators.c
+++ b/src/robofsm/actuators.c
@@ -34,6 +34,20 @@ void act_init(struct robottype_orte_data *ortedata)
        act_lift(0, 0, 1);
 }
 
+void act_camera_recognize(char* target_color)
+{
+        char i = 0;
+
+       orte->camera_control.on = 1;
+
+        /* send RGB color of target to recognize */
+        for (i; i < 2; i++) {
+                orte->camera_control.color[i] = &target_color[i];
+        }
+
+        ORTEPublicationSend(orte->publication_camera_control);
+}
+
 void act_camera_on(void)
 {
        orte->camera_control.on = 1;

diff --git a/src/robofsm/actuators.h b/src/robofsm/actuators.h
index 459ba149af069009a4d780bf97b12f3c030d9e5c..5619f7f99479f7f3c8f4680f62581eb596d2f971 100644 (file)
--- a/src/robofsm/actuators.h
+++ b/src/robofsm/actuators.h
@@ -59,6 +59,7 @@ void act_init(struct robottype_orte_data *ortedata);
 
 void act_camera_on(void);
 void act_camera_off(void);
+void act_camera_recognize(char* target_color);
 
 void act_lift(uint16_t req_pos, char speed, char homing);
 void act_jaws(jaws_cmds cmd);

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 96f8783ea443f93debc547cbe4fd545d7b3d74be..66ea66c0161f5453543364c8be49abe9bbc146c6 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -210,6 +210,7 @@ struct robot {
         /* variables for target detection */
         bool target_loaded;
         bool target_valid;
+        char target_color[3];
 
        struct map *map;        /* Map for pathplanning (no locking) */