disp: pucks and lift indication now working

diff --git a/src/disp-4dgl/control_new.4dg b/src/disp-4dgl/control_new.4dg
index 6220af6dee0e7b46623e9444034059f66f233814..bbaadf290051dcb78b929f5c194864b69908cf03 100644 (file)
--- a/src/disp-4dgl/control_new.4dg
+++ b/src/disp-4dgl/control_new.4dg
@@ -4,7 +4,7 @@
 //#inherit "4DGL-System.fnc"
 \r
 #constant MSG_TERM '~'\r
-#constant MSG_BUFF_LEN 88\r
+#constant MSG_BUFF_LEN 90\r
 #constant MSG_POWER 'V'\r
 #constant MSG_POSITION 'P'\r
 #constant MSG_FSM_STATE 'F'\r
@@ -49,7 +49,7 @@ var fsm_state_len;
 var move_fsm_state[20];\r
 var move_fsm_state_len;\r
 \r
-var lift_pos;\r
+var lift_pos[4];\r
 var pucks_nr;\r
 
 func init()\r
@@ -83,7 +83,10 @@ func init()
         idx++;\r
     until(idx == 5);\r
     \r
-    lift_pos:='0';\r
+    lift_pos[0]:='0';\r
+    lift_pos[1]:='0';\r
+    lift_pos[2]:='0';\r
+    lift_pos[3]:='0';\r
     pucks_nr:='0';\r
 endfunc\r
 \r
@@ -424,6 +427,7 @@ func draw_hw()
 endfunc\r
 \r
 func update_pucks()\r
+    gfx_Rectangle(0, 142, 70, 170, BLACK);\r
     txt_Set(FONT_SIZE, FONT4);\r
     txt_MoveCursor(9, 3);           // move the cursor to line 4, column 5\r
     txt_Set(TEXT_COLOUR, LEMONCHIFFON);\r
@@ -440,10 +444,14 @@ func draw_pucks()
 endfunc\r
 \r
 func update_lift()\r
+    gfx_Rectangle(170, 142, 239, 170, BLACK);\r
     txt_Set(FONT_SIZE, FONT4);\r
     txt_MoveCursor(9, 15);           // move the cursor to line 4, column 5\r
     txt_Set(TEXT_COLOUR, LEMONCHIFFON);\r
-    putch(lift_pos);\r
+    putch(lift_pos[0]);\r
+    putch(lift_pos[1]);\r
+    putch(lift_pos[2]);\r
+    putch(lift_pos[3]);\r
 endfunc\r
 \r
 func draw_lift()\r
@@ -489,8 +497,8 @@ func process_msg()
             game_color := RED;\r
         endif\r
         \r
-        lift_pos := msg_rcvd[83];\r
-        pucks_nr := msg_rcvd[86];\r
+        mem_Copy(msg_rcvd+82, lift_pos, 8);\r
+        pucks_nr := msg_rcvd[90];\r
         \r
         draw_voltage33();\r
         draw_voltage50();\r
@@ -549,3 +557,4 @@ endfunc
 \r
 \r
 \r
+\r

diff --git a/src/robofsm/test/display.cc b/src/robofsm/test/display.cc
index 44ecf28139ba422bd9bd73cbc9801c26079923b5..022dd5cc74e09d9306fa1c8b8733c14e065afcf7 100644 (file)
--- a/src/robofsm/test/display.cc
+++ b/src/robofsm/test/display.cc
@@ -46,6 +46,9 @@ int main()
        robot.orte.pwr_voltage.voltage80 = 8.35;
        robot.orte.pwr_voltage.voltageBAT = 12.35;
 
+       robot.pucks_inside = 3;
+       robot.orte.actuator_status.lift_pos=1234;
+
        robot.fsm.main.transition_callback = trans_callback;
        robot.fsm.motion.transition_callback = move_trans_callback;
 

diff --git a/src/uoled/oledlib.cc b/src/uoled/oledlib.cc
index f99680b7ecb863d6d07f4965ac13a90410f4af1a..3d87d9aee8ff7a488965bad887aa2d45ecd5db15 100644 (file)
--- a/src/uoled/oledlib.cc
+++ b/src/uoled/oledlib.cc
@@ -24,7 +24,9 @@ int oled_send_hw_status(uint8_t *buff, int buff_size, uint8_t *status)
        buff[4] = status[3];
        buff[5] = status[4];
        buff[6] = status[5];
-       buff[7] = MSG_TERM;
+       buff[7] = status[6];
+       buff[8] = status[7];
+       buff[9] = MSG_TERM;
        return HW_STATUS_MSG_SIZE;
 }
 
@@ -104,3 +106,27 @@ int oled_send_fsm_state(uint8_t *buff, int buff_size, const char *name, int len,
        return FSM_STATE_MSG_SIZE;
 }
 
+int oled_send_pucks_nr(uint8_t *buff, int buff_size, char nr)
+{
+       if(buff_size < PUCKS_MSG_SIZE)
+               return -1;
+
+       buff[0] = PUCKS_MSG;
+       sprintf((char*)buff+1, "%1d", nr);
+       buff[2] = MSG_TERM;
+
+       return PUCKS_MSG_SIZE;
+}
+
+int oled_send_lift_pos(uint8_t *buff, int buff_size, uint16_t pos)
+{
+       if(buff_size < LIFT_MSG_SIZE)
+               return -1;
+
+       buff[0] = LIFT_MSG;
+       sprintf((char*)buff+1, "%4d", pos);
+       buff[5] = MSG_TERM;
+
+       return LIFT_MSG_SIZE;
+}
+

diff --git a/src/uoled/oledlib.h b/src/uoled/oledlib.h
index ba218db51ea4da6b5f135efe0e4eb925c1a7d016..9f95bebca6cfcf041e4ce20f2646f8efaf9cea95 100644 (file)
--- a/src/uoled/oledlib.h
+++ b/src/uoled/oledlib.h
@@ -9,12 +9,14 @@
 #define MSG_SIZE 31
 #define MSG_TERM '~'
 
-#define HW_STATUS_MSG_SIZE 8
+#define HW_STATUS_MSG_SIZE 10
 #define SWITCH_MODE_MSG_SIZE 4
 #define VOLTAGE_MSG_SIZE 19 
 #define FSM_STATE_MSG_SIZE 22
 #define POSITION_MSG_SIZE 14
 #define COLOR_MSG_SIZE 3
+#define LIFT_MSG_SIZE 6
+#define PUCKS_MSG_SIZE 3
 
 #define VOLTAGE_MSG 'V'
 #define FSM_STATE_MSG 'F'
@@ -22,6 +24,8 @@
 #define COLOR_MSG 164
 #define POSITION_MSG 'P'
 #define MOVE_FSM_STATE_MSG 'M'
+#define LIFT_MSG 'L'
+#define PUCKS_MSG 'p'
 
 #define ACTUATORS 180
 #define GET_READY 181
@@ -80,6 +84,8 @@ int oled_set_color(uint8_t *buff, int buff_size, uint8_t color);
 int oled_send_position(uint8_t *buff, int buff_size, struct est_pos_type *pos);
 int oled_send_fsm_state(uint8_t *buff, int buff_size, const char *name, int len, uint8_t type);
 int oled_send_hw_status(uint8_t *buff, int buff_size, uint8_t *status);
+int oled_send_pucks_nr(uint8_t *buff, int buff_size, char nr);
+int oled_send_lift_pos(uint8_t *buff, int buff_size, uint16_t pos);
 #ifdef __cplusplus
 }
 #endif

diff --git a/src/uoled/uoled.c b/src/uoled/uoled.c
index 3b7a25f5398ed44aaa08b8d40b675838eee89959..c638ee680b801d1c9dea68fd2c973f4da53af9e0 100644 (file)
--- a/src/uoled/uoled.c
+++ b/src/uoled/uoled.c
@@ -25,6 +25,7 @@
 #define DEBUG
 #endif
 
+#define DEBUG
 static struct sercom_data sercom;
 
 struct sercom_data* uoled_init(void(*sighandler)(int)) 
@@ -192,6 +193,18 @@ int uoled_send_status(struct robot *state)
        else
                idx += ret;
 
+       ret = oled_send_lift_pos(msg+idx, sizeof(msg)-idx, state->orte.actuator_status.lift_pos);
+       if(ret==-1)
+               return -1;
+       else
+               idx += ret;
+
+       ret = oled_send_pucks_nr(msg+idx, sizeof(msg)-idx, state->pucks_inside);
+       if(ret==-1)
+               return -1;
+       else
+               idx += ret;
+
 
        ret = uoled_write_cmd(msg, idx);
        if(ret)