cand: Publish status of robot switches

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 12687323a4d4d95709f3c579da52288a23461f61..12f3c6d6b72cd5253d4535a049907241d2fa18a8 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -182,6 +182,11 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
                        orte->robot_cmd.start_condition = frame.data[0];
                        ORTEPublicationSend(orte->publication_robot_cmd);
                        break;
+               case CAN_ROBOT_SWITCHES:
+                       orte->robot_switches.bumper_pressed = !!(frame.data[0] & CAN_SWITCH_BUMPER);
+                       orte->robot_switches.team_color = (frame.data[0] & CAN_SWITCH_COLOR) ? 1 : 0;
+                       ORTEPublicationSend(orte->publication_robot_switches);
+                       break;
 
                /* positioning by odometry */
                case CAN_ODO_DATA:
@@ -377,6 +382,7 @@ int main(int argc, char *argv[])
        robottype_publisher_odo_data_create(&orte, NULL, NULL);
        robottype_publisher_motion_irc_create(&orte, NULL, NULL);
        robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
+       robottype_publisher_robot_switches_create(&orte, NULL, NULL);
        robottype_publisher_vidle_status_create(&orte, NULL, NULL);
        printf("Publishers OK\n");
 

diff --git a/src/common/can_ids.h b/src/common/can_ids.h
index 73d2631d24914c14890191ee575ab5d814536755..fdcf1b7cdc828a7d9acf2d4d462f51af1d156e55 100644 (file)
--- a/src/common/can_ids.h
+++ b/src/common/can_ids.h
@@ -67,7 +67,7 @@
 //#define CAN_ERROR            to_boa(0x48)    // FIXME: (F.J.) rename me, ...
 
 
-
+#define CAN_ROBOT_SWITCHES     to_boa(0x47)
 #define CAN_VIDLE_STATUS       to_boa(0x48)
 #define CAN_VIDLE_CMD          to_per(0x49)
 

diff --git a/src/common/can_msg_def.h b/src/common/can_msg_def.h
index 4f561de689403ae47539805e4c305c9f4ed3cd0f..73ad911f8137840f4da055f917cef033cf8a8ac3 100644 (file)
--- a/src/common/can_msg_def.h
+++ b/src/common/can_msg_def.h
@@ -3,3 +3,6 @@
 #define CAN_VIDLE_INITIALIZING    0x01
 #define CAN_VIDLE_TIMEOUT        0x02
 #define CAN_VIDLE_OUT_OF_BOUNDS   0x04
+
+#define CAN_SWITCH_BUMPER        0x01
+#define CAN_SWITCH_COLOR         0x02

diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index ea68bdde79176ac97c47e44ae3a4ef162431633a..c1c1c9d83b0c0fc5026438b6c6b47c6a94031d2c 100644 (file)
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -62,6 +62,11 @@ struct robot_cmd {
        boolean start_condition;
 };
 
+struct robot_switches {
+       boolean bumper_pressed;
+       octet team_color;
+};
+
 struct can_msg {
     octet data[8];
     octet len;

diff --git a/src/types/robottype.ortegen b/src/types/robottype.ortegen
index 5660ca2100aa65f7280f681e0ae7eb2c726bc471..b32e359f3ea18e59736e2c8bfeb7b8c36a45b256 100644 (file)
--- a/src/types/robottype.ortegen
+++ b/src/types/robottype.ortegen
@@ -17,7 +17,8 @@ type=pwr_alert                topic=pwr_alert
 type=pwr_ctrl          topic=pwr_ctrl
 type=pwr_voltage       topic=pwr_voltage
 type=robot_pos         topic=ref_pos
-type=robot_cmd         topic=robot_cmd
+type=robot_cmd         topic=robot_cmd         deadline=1
+type=robot_switches    topic=robot_switches    deadline=1
 type=camera_result     topic=camera_result
 type=camera_control    topic=camera_control
 type=fsm_state         topic=fsm_main          pubdelay=1