displayd: Show team color

diff --git a/src/displayd/displayd.c b/src/displayd/displayd.c
index 84378f72036d0be3801f566ae36a481741af8c85..c4be3d9ad0c2a99102b5a1a08a8d8d5f3eb10085 100644 (file)
--- a/src/displayd/displayd.c
+++ b/src/displayd/displayd.c
@@ -154,6 +154,28 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
        last_status = status;
 }
 
+void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance,
+                          void *recvCallBackParam)
+{
+       UDE_hw_status_t status = STATUS_FAILED;
+       static char last_color;
+       static struct timespec last_sent;
+       struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       if (instance->team_color != last_color ||
+                           miliseconds_elapsed_since(&last_sent, 1000)) {
+                               uoled_display_color(instance->team_color);
+                               clock_gettime(CLOCK_MONOTONIC, &last_sent);
+                       }
+                       last_color = instance->team_color;
+               case DEADLINE:
+                       status = STATUS_FAILED;
+                       break;
+       }
+}
+
 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
                         void *recvCallBackParam)
 {
@@ -429,6 +451,7 @@ int main(int argc, char *argv[])
        robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
        robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
        robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, &orte);
+       robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, &orte);
 
        printf("displayd: subscribers created OK\n");
 

diff --git a/src/displayd/uoled.c b/src/displayd/uoled.c
index 01c2e46228e368e196863398e1b7b0d75e54f717..dd58b7c5ee5c74e0d0d164b576f91fc05319f857 100644 (file)
--- a/src/displayd/uoled.c
+++ b/src/displayd/uoled.c
@@ -149,6 +149,20 @@ int uoled_display_alive()
        return 0;
 }
 
+int uoled_display_color(int color)
+{
+       int ret;
+       uint8_t msg[MGS_LENGTH_DISPLAY_COLOR];
+       msg[0]=ID_DISPLAY_COLOR;
+       msg[1]=(char)color;
+
+       ret = uoled_write_cmd(msg, MGS_LENGTH_DISPLAY_COLOR);
+       if(ret)
+               return -1;
+
+       return 0;
+}
+
 int uoled_display_voltage(double voltage33, double voltage50, 
                        double voltage80, double voltageBAT)
 {

diff --git a/src/displayd/uoled.h b/src/displayd/uoled.h
index 0f6e2e468581baf8cbfd574d0035507cd937272d..69ab7db7532833d3ddce20f54105908845176093 100644 (file)
--- a/src/displayd/uoled.h
+++ b/src/displayd/uoled.h
@@ -170,12 +170,16 @@ extern "C" {
 //#define ID_DISPLAY_SM_ACT 0x85
 #define ID_DISPLAY_POSITION 0x86
 #define ID_DISPLAY_ALIVE 0x87
+#define ID_DISPLAY_CORNS 0x88
+#define ID_DISPLAY_COLOR 0x89
 
 #define MGS_LENGTH_DISPLAY_STATUS 2
 #define MGS_LENGTH_DISPLAY_SM 21
 #define MGS_LENGTH_DISPLAY_VOLTAGE 18
 #define MGS_LENGTH_DISPLAY_POSITION 10
 #define MGS_LENGTH_DISPLAY_ALIVE 1
+#define MGS_LENGTH_DISPLAY_CORNS 3
+#define MGS_LENGTH_DISPLAY_COLOR 2
 
 #define TRESHOLDS_VOLTAGE33 3.2
 #define TRESHOLDS_VOLTAGE50 4.9
@@ -326,6 +330,9 @@ int uoled_display_voltage(double voltage33, double voltage50,
  */
 int uoled_display_position(double x, double y, double phi);
 
+int uoled_display_color(int color);
+
+
 #ifdef __cplusplus
 }
 #endif