disp: added main fsm state vizualization

diff --git a/src/disp-4dgl/control_screen.4dg b/src/disp-4dgl/control_screen.4dg
index afd781c43137532411965bae1ea9daa5d2dd843e..2f77684dcd547f5eee577cd81b6381311e6f1f66 100644 (file)
--- a/src/disp-4dgl/control_screen.4dg
+++ b/src/disp-4dgl/control_screen.4dg
@@ -190,11 +190,11 @@ func color_bt(var state)
 endfunc\r
 \r
 func restart_bt(var state)\r
-    gfx_Button(state, X_OFFSET, bt_y[BT_RESTART], GRAY, YELLOW, FONT2, 2, 2, "    RESTART   ");\r
+    gfx_Button(state, X_OFFSET, bt_y[BT_RESTART], GRAY, YELLOW, FONT2, 2, 2, "  RESTART ALL ");\r
 endfunc\r
 \r
 func init_bt(var state)\r
-    gfx_Button(state, X_OFFSET, bt_y[BT_INIT], GRAY, YELLOW, FONT2, 2, 2, "     INIT     ");\r
+    gfx_Button(state, X_OFFSET, bt_y[BT_INIT], GRAY, YELLOW, FONT2, 2, 2, "     EXIT     ");\r
 endfunc\r
 \r
 func status_bt(var state)\r
@@ -429,3 +429,4 @@ endfunc
 \r
 \r
 \r
+\r

diff --git a/src/disp-4dgl/status_screen_hacked.4dg b/src/disp-4dgl/status_screen_hacked.4dg
new file mode 100644 (file)
index 0000000..3dad6a2
--- /dev/null
+++ b/src/disp-4dgl/status_screen_hacked.4dg
@@ -0,0 +1,706 @@
+#platform "uOLED-32028-PMD3T"
+
+#inherit "4DGL_16bitColours.fnc"\r
+//#inherit "4DGL-System.fnc"
+\r
+#constant MSG_TERM '~'\r
+#constant MSG_BUFF_LEN 123\r
+#constant MSG_POWER 'V'\r
+#constant MSG_POSITION 'P'\r
+#constant MSG_FSM_STATE 'F'\r
+#constant MSG_MOVE_FSM_STATE 'M'\r
+\r
+#constant MSG_START 150\r
+#constant MSG_TERM_LF 10\r
+\r
+#constant MSG_SWITCH_TO_STATUS_DONE 185\r
+\r
+#constant MSG_STATUS 'S'\r
+
+// Indices to the voltage_status array
+#constant VOLTAGE33 0
+#constant VOLTAGE50 1
+#constant VOLTAGE80 2
+#constant VOLTAGE_BAT 3
+
+/*Voltage branch OK indication*/
+#constant VOLTAGE_OK 1\r
+\r
+#constant HW_STATUS_OK 1\r
+#constant HW_STATUS_WARN 2\r
+\r
+#constant MSG_CONTROL_REQ 184\r
+#constant MSG_SWITCH_REP 200\r
+\r
+#constant HW1 1\r
+#constant HW2 2\r
+#constant HW3 3\r
+#constant HW4 4\r
+#constant HW5 5\r
+#constant HW6 6\r
+
+var msg_buff[MSG_BUFF_LEN]; // serial RX buffer\r
+var msg_rcvd[MSG_BUFF_LEN]; // serial RX buffer\r
+var out_buff[3];
+
+var voltage33[4];
+var voltage50[4];
+var voltage80[4];
+var voltageBAT[5];
+
+var voltage_status[4]; // status of individual voltage branches
+
+var position[8];
+var game_color;
+
+var act_fsm_state[20];
+var move_fsm_state[20];\r
+var main_fsm_state[20];\r
+\r
+var lift_pos[4];\r
+var pucks_nr;\r
+var game_conf;\r
+\r
+var hw_status[8];\r
+
+func init()\r
+    var idx;\r
+    \r
+    game_color:=BLACK;\r
+    \r
+    idx:=0;\r
+    repeat\r
+        position[idx]:=' ';\r
+        idx++;\r
+    until(idx == 8);\r
+    idx:=0;\r
+    repeat\r
+        voltage33[idx]:=' ';\r
+        idx++;\r
+    until(idx == 4);\r
+    idx:=0;\r
+    repeat\r
+        voltage50[idx]:=' ';\r
+        idx++;\r
+    until(idx == 4);\r
+    idx:=0;\r
+    repeat\r
+        voltage80[idx]:=' ';\r
+        idx++;\r
+    until(idx == 4);\r
+    idx:=0;\r
+    repeat\r
+        voltageBAT[idx]:=' ';\r
+        idx++;\r
+    until(idx == 5);\r
+    \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
+    game_conf:='1';\r
+endfunc\r
+\r
+
+func clear_screen()
+    gfx_Rectangle(0, 0, 239, 319, BLACK);   
+    return 0;
+endfunc\r
+\r
+func bt_control(var state)\r
+    gfx_Button(state, 2, 290, GRAY, WHITE, FONT3, 1, 1, "           CONTROL          ");\r
+endfunc 
+
+func draw_voltage33() // draw the 3.3 V voltage status indicator
+    var col_rect;
+
+    if(voltage_status[VOLTAGE33]==VOLTAGE_OK)
+        col_rect := RED;
+    else
+        col_rect := GREEN;
+    endif
+
+    gfx_Rectangle(0,10,60,50,col_rect);
+    gfx_Line(59, 10, 59, 50, BLACK);
+    txt_Set(TEXT_COLOUR, WHITE);
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(1,1);
+    putstr("3,30V");
+    txt_Set(FONT_SIZE, FONT4);
+    txt_MoveCursor(2,0);
+    putch(voltage33[0]);
+    putch(voltage33[1]);
+    putch(voltage33[2]);
+    putch(voltage33[3]);
+    return 0;
+endfunc
+
+func draw_voltage50()
+    var col_rect;
+
+    if(voltage_status[VOLTAGE50]==VOLTAGE_OK)
+        col_rect := RED;
+    else
+        col_rect := GREEN;
+    endif
+
+    gfx_Rectangle(60,10,120,50,col_rect);
+    gfx_Line(119, 10, 119, 50, BLACK);
+    txt_Set(TEXT_COLOUR, WHITE);
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(1,9);
+    putstr("5,00V");
+    txt_Set(FONT_SIZE, FONT4);
+    txt_MoveCursor(2,5);
+    putch(voltage50[0]);
+    putch(voltage50[1]);
+    putch(voltage50[2]);
+    putch(voltage50[3]);
+    return 0;
+endfunc
+
+func draw_voltage80()
+    var col_rect;
+
+    if(voltage_status[VOLTAGE80]==VOLTAGE_OK)
+        col_rect := RED;
+    else
+        col_rect := GREEN;
+    endif
+
+    gfx_Rectangle(120,10,180,50,col_rect);
+    gfx_Line(179, 10, 179, 50, BLACK);
+    txt_Set(TEXT_COLOUR, WHITE);
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(1,17);
+    putstr("8,00V");
+    txt_Set(FONT_SIZE, FONT4);
+    txt_MoveCursor(2,10);
+    putch(voltage80[0]);
+    putch(voltage80[1]);
+    putch(voltage80[2]);
+    putch(voltage80[3]);
+    return 0;
+endfunc
+
+func draw_voltageBAT()
+    var col_rect;
+
+    if(voltage_status[VOLTAGE_BAT]==VOLTAGE_OK)
+        col_rect := RED;
+    else
+        col_rect := GREEN;
+    endif
+
+    gfx_Rectangle(180,10,240,50,col_rect);
+    txt_Set(TEXT_COLOUR, WHITE);
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(1,25);
+    putstr("BATT");
+    txt_Set(FONT_SIZE, FONT4);
+    txt_MoveCursor(2,15);
+    putch(voltageBAT[0]);
+    putch(voltageBAT[1]);
+    putch(voltageBAT[2]);
+    putch(voltageBAT[3]);
+    putch(voltageBAT[4]);
+    return 0;
+endfunc
+
+func draw_voltage()
+    putstr("VOLTAGE");
+    draw_voltage33();
+    draw_voltage50();
+    draw_voltage80();
+    draw_voltageBAT();
+endfunc
+
+func draw_position()
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(21, 6);     
+    txt_Set(TEXT_COLOUR, GRAY);
+    
+    putstr("POSITION AND COLOR");
+
+    gfx_Rectangle(0,266,239,289,game_color);
+    txt_Set(FONT_SIZE, FONT4);
+    txt_Set(TEXT_COLOUR, WHITE);
+    txt_MoveCursor(17,0);
+    putstr("X,Y: ");
+    putch(position[0]);
+    putch(position[1]);
+    putch(position[2]);
+    putch(position[3]);
+    putch(' ');
+    putch(position[4]);
+    putch(position[5]);
+    putch(position[6]);
+    putch(position[7]);
+    putstr(" [m]");
+    return 0;
+endfunc\r
+
+func draw_fsm_state()
+    var idx;
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(5, 6);           // move the cursor to line 4, column 5
+    txt_Set(TEXT_COLOUR, GRAY);
+    
+    putstr("CURRENT FSM STATE");
+    return 0;
+endfunc
+
+func update_act_fsm_state()
+    var idx;
+    gfx_Rectangle(0, 99, 239, 112, BLACK);
+    txt_Set(FONT_SIZE, FONT4);
+    txt_MoveCursor(6, 0);           // move the cursor to line 4, column 5
+    txt_Set(TEXT_COLOUR, WHITE);
+    idx:=0;
+    repeat
+        putch(act_fsm_state[idx]);
+        idx++;
+    until(idx==20)
+    return 0;
+endfunc
+
+func update_move_fsm_state()\r
+    var idx;\r
+    gfx_Rectangle(0, 128, 239, 141, BLACK);\r
+    txt_Set(FONT_SIZE, FONT4);\r
+    txt_MoveCursor(7, 0);           // move the cursor to line 4, column 5\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    idx:=0;\r
+    repeat\r
+        putch(move_fsm_state[idx]);\r
+        idx++;\r
+    until(idx==20)\r
+    return 0;\r
+endfunc\r
+\r
+func update_main_fsm_state()\r
+    var idx;\r
+    gfx_Rectangle(0, 70, 239, 99, BLACK);\r
+    txt_Set(FONT_SIZE, FONT4);\r
+    txt_MoveCursor(5, 0);           // move the cursor to line 4, column 5\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    idx:=0;\r
+    repeat\r
+        putch(main_fsm_state[idx]);\r
+        idx++;\r
+    until(idx==20)\r
+    return 0;\r
+endfunc\r
+\r
+func draw_hw1(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(0,193,59,218,GREEN);\r
+    else\r
+        if(status==HW_STATUS_WARN)
+            gfx_Rectangle(0,193,59,218,ORANGE);\r
+        else
+            gfx_Rectangle(0,193,59,218,RED);\r
+        endif
+    endif\r
+\r
+    txt_MoveCursor(17,3);\r
+    putstr("MOT");\r
+endfunc\r
+\r
+func draw_hw2(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(61,193,120,218,GREEN);\r
+    else\r
+           if(status==HW_STATUS_WARN)
+            gfx_Rectangle(61,193,120,218,ORANGE);\r
+           else
+            gfx_Rectangle(61,193,120,218,RED);\r
+           endif
+    endif\r
+    txt_MoveCursor(17,10);\r
+    putstr("UZV");\r
+endfunc\r
+\r
+func draw_hw3(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(122,193,181,218,GREEN);\r
+    else\r
+           if(status==HW_STATUS_WARN)
+            gfx_Rectangle(122,193,181,218,ORANGE);\r
+           else
+            gfx_Rectangle(122,193,181,218,RED);\r
+           endif
+    endif\r
+\r
+    txt_MoveCursor(17,17);\r
+    putstr("LFT");\r
+endfunc\r
+\r
+func draw_hw3a(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(183,193,239,218,GREEN);\r
+    else\r
+           if(status==HW_STATUS_WARN)
+            gfx_Rectangle(183,193,239,218,ORANGE);\r
+           else
+            gfx_Rectangle(183,193,239,218,RED);\r
+           endif
+    endif\r
+\r
+    txt_MoveCursor(17,25);\r
+    putstr("CHE");\r
+endfunc\r
+\r
+func draw_hw4(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(0,220,59,245,GREEN);\r
+    else\r
+           if(status==HW_STATUS_WARN)
+            gfx_Rectangle(0,220,59,245,ORANGE);\r
+           else
+            gfx_Rectangle(0,220,59,245,RED);\r
+           endif
+    endif\r
+    \r
+    txt_MoveCursor(19,3);\r
+    putstr("CAM");\r
+endfunc\r
+\r
+func draw_hw5(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(61,220,120,245,GREEN);\r
+    else\r
+           if(status==HW_STATUS_WARN)
+            gfx_Rectangle(61,220,120,245,ORANGE);\r
+           else
+            gfx_Rectangle(61,220,120,245,RED);\r
+           endif
+    endif\r
+    \r
+    txt_MoveCursor(19,10);\r
+    putstr("HOK");\r
+endfunc\r
+\r
+func draw_hw6(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(122,220,181,245,GREEN);\r
+    else\r
+           if(status==HW_STATUS_WARN)
+            gfx_Rectangle(122,220,181,245,ORANGE);\r
+           else
+            gfx_Rectangle(122,220,181,245,RED);\r
+           endif
+    endif\r
+    \r
+    txt_MoveCursor(19,17);\r
+    putstr("PWR");\r
+endfunc\r
+\r
+func draw_hw6a(var status)\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+    \r
+    if(status==HW_STATUS_OK)\r
+        gfx_Rectangle(183,220,239,245,GREEN);\r
+    else\r
+        if(status==HW_STATUS_WARN)\r
+            gfx_Rectangle(183,220,239,245,ORANGE);\r
+        else\r
+            gfx_Rectangle(183,220,239,245,RED);\r
+        endif\r
+    endif\r
+    \r
+    txt_MoveCursor(19,25);\r
+    putstr("STA");\r
+endfunc\r
+\r
+func draw_hw()\r
+    txt_Set(FONT_SIZE, FONT3);\r
+\r
+    txt_Set(TEXT_COLOUR, WHITE);\r
+\r
+    draw_hw1(HW_STATUS_OK);\r
+    draw_hw2(HW_STATUS_OK);\r
+    draw_hw3(HW_STATUS_OK);\r
+    draw_hw3a(HW_STATUS_OK);\r
+    draw_hw4(HW_STATUS_OK);\r
+    draw_hw5(HW_STATUS_OK);\r
+    draw_hw6(HW_STATUS_OK);\r
+    draw_hw6a(HW_STATUS_OK);\r
+    return 0;\r
+endfunc\r
+\r
+func update_pucks()\r
+    gfx_Rectangle(0, 170, 50, 190, BLACK);\r
+    txt_Set(FONT_SIZE, FONT4);\r
+    txt_MoveCursor(11, 3);           \r
+    txt_Set(TEXT_COLOUR, LEMONCHIFFON);\r
+    putch(pucks_nr);\r
+endfunc\r
+\r
+func draw_pucks()\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_MoveCursor(13, 1);           \r
+    txt_Set(TEXT_COLOUR, GRAY);\r
+    \r
+    putstr("PUCKS nr.");\r
+    update_pucks();\r
+endfunc\r
+\r
+func update_lift()\r
+    gfx_Rectangle(170, 170, 239, 190, BLACK);\r
+    txt_Set(FONT_SIZE, FONT4);\r
+    txt_MoveCursor(11, 15);           \r
+    txt_Set(TEXT_COLOUR, LEMONCHIFFON);\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
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_MoveCursor(13, 20);         \r
+    txt_Set(TEXT_COLOUR, GRAY);\r
+    \r
+    putstr("LIFT pos.");\r
+    update_lift();\r
+endfunc\r
+\r
+func update_conf()\r
+    gfx_Rectangle(70, 170, 140, 190, BLACK);\r
+    txt_Set(FONT_SIZE, FONT4);\r
+    txt_MoveCursor(11, 9);           // move the cursor to line 4, column 5\r
+    txt_Set(TEXT_COLOUR, LEMONCHIFFON);\r
+    putch(game_conf);\r
+endfunc\r
+\r
+func draw_conf()\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_MoveCursor(13, 10);           // move the cursor to line 4, column 5\r
+    txt_Set(TEXT_COLOUR, GRAY);\r
+    \r
+    putstr("GAME conf.");\r
+    update_conf();\r
+endfunc\r
+    
+func draw_status() 
+    clear_screen();
+    txt_Set(FONT_SIZE, FONT3);
+    txt_MoveCursor(0, 12);           // move the cursor to line 4, column 5
+    txt_Set(TEXT_COLOUR, GRAY);
+    draw_voltage(); \r
+    draw_fsm_state(); \r
+    draw_pucks();\r
+    draw_lift();
+    draw_position();\r
+    draw_hw();\r
+    draw_conf();\r
+    bt_control(UP);
+    return 0;
+endfunc\r
+\r
+func update_hw()\r
+    draw_hw1(hw_status[0]);\r
+    draw_hw2(hw_status[1]);\r
+    draw_hw3(hw_status[2]);\r
+    draw_hw3a(hw_status[3]);\r
+    draw_hw4(hw_status[4]);\r
+    draw_hw5(hw_status[5]);\r
+    draw_hw6(hw_status[6]);\r
+    draw_hw6a(hw_status[7]);\r
+endfunc\r
+\r
+func load_control()\r
+    clear_screen();\r
+    file_Run("CTRL.4XE",0);\r
+endfunc
+
+/*Process the message received on serial line*/
+func process_msg()\r
+    var idx;\r
+    if(msg_rcvd[0]==MSG_STATUS)\r
+        mem_Copy(msg_rcvd+2, voltage33, 8);\r
+        mem_Copy(msg_rcvd+6, voltage50, 8);\r
+        mem_Copy(msg_rcvd+10, voltage80, 8);\r
+        mem_Copy(msg_rcvd+14, voltageBAT, 10);\r
+        \r
+        mem_Copy(msg_rcvd+21, position, 16);\r
+        \r
+        mem_Copy(msg_rcvd+35, act_fsm_state, 40);\r
+        mem_Copy(msg_rcvd+57, move_fsm_state, 40);\r
+        \r
+        if(msg_rcvd[79]==0)\r
+            game_color := GREEN;\r
+        else\r
+            game_color := RED;\r
+        endif\r
+        \r
+        mem_Copy(msg_rcvd+82, lift_pos, 8);\r
+        pucks_nr := msg_rcvd[88];\r
+        \r
+        mem_Copy(msg_rcvd+91, hw_status, 16);\r
+        //mem_Copy(msg_rcvd+101, game_conf, 2);\r
+        game_conf:=msg_rcvd[101];\r
+\r
+        mem_Copy(msg_rcvd+105, main_fsm_state, 40);\r
+        \r
+        draw_voltage33();\r
+        draw_voltage50();\r
+        draw_voltage80();\r
+        draw_voltageBAT();\r
+        draw_position();\r
+        update_main_fsm_state();\r
+        update_act_fsm_state();\r
+        update_move_fsm_state();\r
+        update_lift();\r
+        update_pucks();\r
+        update_conf();\r
+        update_hw();\r
+        \r
+        \r
+    endif\r
+    if(msg_rcvd[0]==200)\r
+        if(msg_rcvd[2]==2)\r
+            load_control();\r
+        endif\r
+    endif\r
+        
+endfunc
+
+func get_msg()
+    var in, idx, bad, msg_len;
+
+    idx:=0;
+    bad:=0;    
+    in:=serin();
+    
+    repeat
+        in:=serin();
+        if(in!=-1)
+            msg_rcvd[idx] := in;
+            idx++;
+        else
+            break;
+        endif\r
+    forever
+\r
+    msg_len := idx;\r
+    com_Init(msg_buff, MSG_BUFF_LEN, ':');
+
+    process_msg();
+    return 0;
+endfunc    \r
+\r
+func act_control()    \r
+    var idx;\r
+    \r
+    out_buff[0]:=MSG_START;\r
+    out_buff[1]:=MSG_CONTROL_REQ;\r
+    out_buff[2]:=MSG_TERM_LF;\r
+    \r
+    idx:=0;\r
+    repeat\r
+        serout(out_buff[idx]);\r
+        idx++;\r
+    until(idx==3)    \r
+endfunc
+\r
+func touchscreen() \r
+    var col, state, x, y;\r
+    state := touch_Get(TOUCH_STATUS);               // get touchscreen status\r
+        \r
+        if(state == NOTOUCH)\r
+            return;\r
+        endif\r
+        //-----------------------------------------------------------------------------------------\r
+        if(state == TOUCH_PRESS)                        // if there's a press\r
+            x := touch_Get(TOUCH_GETX);                          \r
+            y := touch_Get(TOUCH_GETY);\r
+            if(y>290)\r
+                bt_control(DOWN);\r
+            endif\r
+        endif\r
+        if(state == TOUCH_RELEASE)                        // if there's a press\r
+            x := touch_Get(TOUCH_GETX);                          \r
+            y := touch_Get(TOUCH_GETY);\r
+            if(y>290)\r
+                bt_control(UP);\r
+                act_control();\r
+            endif\r
+        endif\r
+            \r
+endfunc\r
+
+func main()\r
+    var idx;
+    setbaud(8); // 19200 baud
+    com_Init(msg_buff, MSG_BUFF_LEN, ':');\r
+    \r
+    out_buff[0]:=MSG_START;\r
+    out_buff[1]:=MSG_SWITCH_TO_STATUS_DONE;\r
+    out_buff[2]:=MSG_TERM_LF;\r
+    idx:=0;\r
+    repeat\r
+        serout(out_buff[idx]);\r
+        idx++;\r
+    until(idx==3)  \r
+
+
+    touch_Set(TOUCH_ENABLE); // enable TS
+    touch_Set(TOUCH_REGIONDEFAULT); // reset touch area to fullscreen\r
+    \r
+    init();\r
+    \r
+    draw_status();
+    repeat // the main application loop
+       touchscreen();\r
+        if(com_Full()==1)\r
+            get_msg();\r
+        endif
+    forever
+endfunc
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+\r

diff --git a/src/uoled/uoled.c b/src/uoled/uoled.c
index 42ccbdf1156e6f8dafcd4604c67ad9e1defa2ef0..8cfa011a584710af3afc5f972850aaa692ddf9c6 100644 (file)
--- a/src/uoled/uoled.c
+++ b/src/uoled/uoled.c
@@ -181,7 +181,7 @@ int uoled_send_control(struct robot *state)
 int uoled_send_status(struct robot *state)
 {
        int ret;
-       uint8_t msg[110];
+       uint8_t msg[130];
        int idx = 0;
 
        msg[0] = MSG_START;
@@ -245,6 +245,12 @@ int uoled_send_status(struct robot *state)
        else
                idx += ret;
 
+       ret = oled_send_fsm_state(msg+idx, sizeof(msg)-idx, (state->orte.fsm_main.state_name), 
+                       strlen(state->orte.fsm_main.state_name), MOVE_FSM_STATE_MSG);
+       if(ret==-1)
+               return -1;
+       else
+               idx += ret;
 
        ret = uoled_write_cmd(msg, idx);
        if(ret)