display: both display modes now work correctly, switching finaly works too

diff --git a/src/disp-4dgl/control.4dg b/src/disp-4dgl/control.4dg
index 72f25322096d0edbad9adba697e72f4886b19459..75cb5d7df4a0febb868f63155a109365f78977fb 100644 (file)
--- a/src/disp-4dgl/control.4dg
+++ b/src/disp-4dgl/control.4dg
@@ -14,11 +14,24 @@
 \r
 #constant MSG_START 150\r
 \r
+#constant MSG_VOLTAGE 160\r
+#constant MSG_FSM_STATE 161\r
+#constant MSG_BALLS 162\r
+#constant MSG_HW_STATUS 163\r
+#constant MSG_COLOR 164\r
+#constant MSG_POSITION 165\r
+#constant CHANGE_MODE_REP 200\r
+#constant CHANGE_MODE_REQ 201\r
+#constant CHANGE_MODE_STATUS 202\r
+#constant CHANGE_MODE_CONTROL 203\r
+\r
 #constant MSG_ACTUATORS 180\r
 #constant MSG_GET_READY 181\r
 #constant MSG_ROBOT_START 182\r
 #constant MSG_SWITCH_TO_STATUS 183\r
 #constant MSG_SWITCH_TO_CONTROL 184\r
+#constant MSG_SWITCH_TO_STATUS_DONE 185\r
+#constant MSG_SWITCH_TO_CONTROL_DONE 186\r
 \r
 #constant MSG_LBRUSH_SERVO 1\r
 #constant MSG_RBRUSH_SERVO 2\r
@@ -37,6 +50,7 @@
 #constant TEXTHEIGHT 1    \r
 #constant TXT_BT_1 $"CONS"\r
 #constant TXT_BT_STATUS $"           STATUS           "\r
+#constant TXT_BT_CONTROL $"           CONTROL          "\r
 #constant TXT_BT_3 $" MAP "\r
 #constant TXT_BT_4 $" MISC "\r
 \r
@@ -78,6 +92,10 @@
 #constant BT_STATUS_X                10\r
 #constant BT_STATUS_END_X        11\r
 \r
+#constant VOLTAGE33 0\r
+#constant VOLTAGE50 1\r
+#constant VOLTAGE80 2\r
+#constant VOLTAGE_BAT 3\r
 \r
 var msg_buff[MSG_BUFF_LEN];\r
 var bt_x[20];\r
@@ -88,12 +106,17 @@ var voltage33[4];
 var voltage50[4];\r
 var voltage80[4];\r
 var voltageBAT[5];\r
+\r
+var voltage_status[4];\r
     \r
 var position[8];\r
-var color;\r
+var game_color;\r
 var fsm_state[41];\r
 var fsm_state_len;\r
 \r
+var isStatus;\r
+var isControl;\r
+\r
 var out_msg_buff[10];\r
 \r
 func clear_screen()\r
@@ -137,15 +160,15 @@ func bt_start(var state, var color)
     gfx_Button(state, bt_x[BT_START_X], bt_y[BT_START_Y], color, WHITE, FONT4, TEXTWIDTH, TEXTHEIGHT, TXT_BT_START);\r
 endfunc\r
 \r
-func bt_status(var state, var color)\r
-    gfx_Button(state, bt_x[BT_STATUS_X], bt_y[BT_STATUS_Y], color, WHITE, FONT3, TEXTWIDTH, TEXTHEIGHT, TXT_BT_STATUS);\r
+func bt_status(var state, var color, var text)\r
+    gfx_Button(state, bt_x[BT_STATUS_X], bt_y[BT_STATUS_Y], color, WHITE, FONT3, TEXTWIDTH, TEXTHEIGHT, text);\r
 endfunc\r
 \r
 //----------------------------------------------------------------------------------------------------\r
-func draw_voltage33(var status, var value)\r
+func draw_voltage33()\r
     var col_rect;\r
 \r
-    if(status==VOLTAGE_OK)\r
+    if(voltage_status[VOLTAGE33]==VOLTAGE_OK)\r
         col_rect := RED;\r
     else\r
         col_rect := GREEN;\r
@@ -166,10 +189,10 @@ func draw_voltage33(var status, var value)
     return 0;\r
 endfunc\r
 \r
-func draw_voltage50(var status, var value)\r
+func draw_voltage50()\r
     var col_rect;\r
 \r
-    if(status==VOLTAGE_OK)\r
+    if(voltage_status[VOLTAGE50]==VOLTAGE_OK)\r
         col_rect := RED;\r
     else\r
         col_rect := GREEN;\r
@@ -190,10 +213,10 @@ func draw_voltage50(var status, var value)
     return 0;\r
 endfunc\r
 \r
-func draw_voltage80(var status, var value)\r
+func draw_voltage80()\r
     var col_rect;\r
 \r
-    if(status==VOLTAGE_OK)\r
+    if(voltage_status[VOLTAGE80]==VOLTAGE_OK)\r
         col_rect := RED;\r
     else\r
         col_rect := GREEN;\r
@@ -214,10 +237,10 @@ func draw_voltage80(var status, var value)
     return 0;\r
 endfunc\r
 \r
-func draw_voltageBAT(var status, var value)\r
+func draw_voltageBAT()\r
     var col_rect;\r
 \r
-    if(status==VOLTAGE_OK)\r
+    if(voltage_status[VOLTAGE_BAT]==VOLTAGE_OK)\r
         col_rect := RED;\r
     else\r
         col_rect := GREEN;\r
@@ -296,7 +319,7 @@ func draw_position()
     \r
     putstr("POSITION AND COLOR");\r
 \r
-    gfx_Rectangle(0,266,239,289,color);\r
+    gfx_Rectangle(0,266,239,289,game_color);\r
     txt_Set(FONT_SIZE, FONT4);\r
     txt_Set(TEXT_COLOUR, WHITE);\r
     txt_MoveCursor(17,0);\r
@@ -330,28 +353,6 @@ func draw_hw1()
     gfx_Rectangle(161,220,240,245,GREEN);\r
     return 0;\r
 endfunc\r
-    \r
-func draw_status() \r
-    clear_screen();\r
-    txt_Set(FONT_SIZE, FONT3);\r
-    txt_MoveCursor(0, 12);           // move the cursor to line 4, column 5\r
-    txt_Set(TEXT_COLOUR, RED);\r
-    \r
-    putstr("VOLTAGE");\r
-    draw_voltage33(0, 1);\r
-    draw_voltage50(0, 1);\r
-    draw_voltage80(0, 1);\r
-    draw_voltageBAT(0, 1);\r
-    \r
-    fsm_state_len := 1;\r
-    draw_fsm_state();\r
-\r
-    draw_balls();\r
-    draw_carousel_pos(3, 2);\r
-    draw_hw1();\r
-    draw_position();\r
-    return 0;\r
-endfunc\r
 \r
 //-------------------------------------------------------------------------------------------------\r
 \r
@@ -420,23 +421,52 @@ func switch_to_status()
     until(idx==len)\r
 endfunc\r
 \r
-func get_msg()\r
-    var in, idx, bad;\r
+func switch_to_status_done()\r
+    var len,idx;\r
+    \r
+    out_msg_buff[0] := MSG_START;\r
+    out_msg_buff[1] := MSG_SWITCH_TO_STATUS_DONE;\r
+    out_msg_buff[2] := MSG_TERM_LF;\r
 \r
-    idx:=0;\r
-    bad:=0;\r
+    len := 3;\r
+    idx := 0;\r
+    \r
+    repeat\r
+        serout(out_msg_buff[idx]);\r
+        idx++;\r
+    until(idx==len)\r
+endfunc\r
+\r
+func switch_to_control()\r
+    var len,idx;\r
+    \r
+    out_msg_buff[0] := MSG_START;\r
+    out_msg_buff[1] := MSG_SWITCH_TO_CONTROL;\r
+    out_msg_buff[2] := MSG_TERM_LF;\r
 \r
+    len := 3;\r
+    idx := 0;\r
+    \r
     repeat\r
-        in:=serin();\r
-        if(in!=-1)\r
-            msg_buff[idx] := in;\r
-            idx++;\r
-        endif\r
-    until((in==MSG_TERM)||(idx==MSG_BUFF_LEN))\r
-//    serout(5);\r
-//    serout(10);\r
-    msg_len := idx;\r
-    return 0;\r
+        serout(out_msg_buff[idx]);\r
+        idx++;\r
+    until(idx==len)\r
+endfunc\r
+\r
+func switch_to_control_done()\r
+    var len,idx;\r
+    \r
+    out_msg_buff[0] := MSG_START;\r
+    out_msg_buff[1] := MSG_SWITCH_TO_CONTROL_DONE;\r
+    out_msg_buff[2] := MSG_TERM_LF;\r
+\r
+    len := 3;\r
+    idx := 0;\r
+    \r
+    repeat\r
+        serout(out_msg_buff[idx]);\r
+        idx++;\r
+    until(idx==len)\r
 endfunc\r
 \r
 func draw_ctrl_titles()\r
@@ -453,7 +483,6 @@ func draw_ctrl_titles()
     putstr("ROBOT CONTROL");\r
 endfunc\r
 \r
-\r
 func draw_servos_buttons()\r
     bt_x[BT_X_0] := 0;\r
     bt_y[BT_S_ROW1_Y] := 18;\r
@@ -508,13 +537,147 @@ func draw_status_button()
     bt_x[BT_STATUS_X] := 0;\r
     bt_y[BT_STATUS_Y] := 290;\r
     \r
-    bt_status(UP, GRAY);\r
+    bt_status(UP, GRAY, TXT_BT_STATUS);\r
     \r
     bt_x[BT_STATUS_END_X] := gfx_Get(2);\r
     bt_y[BT_STATUS_END_Y] := gfx_Get(3);\r
 endfunc\r
 \r
+func draw_control_button()\r
+    bt_x[BT_STATUS_X] := 0;\r
+    bt_y[BT_STATUS_Y] := 290;\r
+    \r
+    bt_status(UP, GRAY, TXT_BT_CONTROL);\r
+    \r
+    bt_x[BT_STATUS_END_X] := gfx_Get(2);\r
+    bt_y[BT_STATUS_END_Y] := gfx_Get(3);\r
+endfunc\r
+\r
+func draw_status() \r
+    clear_screen();\r
+    txt_Set(FONT_SIZE, FONT3);\r
+    txt_MoveCursor(0, 12);           // move the cursor to line 4, column 5\r
+    txt_Set(TEXT_COLOUR, RED);\r
+    \r
+    putstr("VOLTAGE");\r
+    draw_voltage33();\r
+    draw_voltage50();\r
+    draw_voltage80();\r
+    draw_voltageBAT();\r
+    \r
+    fsm_state_len := 1;\r
+    draw_fsm_state();\r
+\r
+    draw_balls();\r
+    draw_carousel_pos(3, 2);\r
+    draw_hw1();\r
+    draw_position();\r
+    draw_control_button();\r
+    return 0;\r
+endfunc\r
+\r
+func draw_control()\r
+    clear_screen();\r
+    draw_ctrl_titles();\r
+    draw_servos_buttons();\r
+    draw_drives_buttons();\r
+    draw_robot_control_buttons();\r
+    draw_status_button();\r
+endfunc\r
+\r
+func process_msg()\r
+    var idx;\r
+    \r
+    if(msg_buff[0]==CHANGE_MODE_REP)\r
+        if(msg_buff[1]==CHANGE_MODE_STATUS)\r
+            draw_status();\r
+            isControl:=0;\r
+            isStatus:=1;            \r
+            switch_to_status_done();\r
+        endif\r
+        if(msg_buff[1]==CHANGE_MODE_CONTROL)\r
+            draw_control();\r
+            isStatus:=0;\r
+            isControl:=1;\r
+            switch_to_control_done();\r
+        endif\r
+    endif\r
+    if((msg_buff[0]==MSG_VOLTAGE)&&(isStatus==1))\r
+        \r
+        voltage33[0]:=msg_buff[1];\r
+        voltage33[1]:=msg_buff[2];\r
+        voltage33[2]:=msg_buff[3];\r
+        voltage33[3]:=msg_buff[4];\r
+        \r
+        voltage50[0]:=msg_buff[5];\r
+        voltage50[1]:=msg_buff[6];\r
+        voltage50[2]:=msg_buff[7];\r
+        voltage50[3]:=msg_buff[8];\r
+        \r
+        voltage80[0]:=msg_buff[9];\r
+        voltage80[1]:=msg_buff[10];\r
+        voltage80[2]:=msg_buff[11];\r
+        voltage80[3]:=msg_buff[12];\r
+        \r
+        voltageBAT[0]:=msg_buff[13];\r
+        voltageBAT[1]:=msg_buff[14];\r
+        voltageBAT[2]:=msg_buff[15];\r
+        voltageBAT[3]:=msg_buff[16];\r
+        voltageBAT[4]:=msg_buff[17];\r
+        \r
+        draw_voltage33();\r
+        draw_voltage50();\r
+        draw_voltage80();\r
+        draw_voltageBAT();\r
+\r
+    endif\r
+    if((msg_buff[0]==MSG_POSITION)&&(isStatus==1))\r
+        idx:=1;\r
+        repeat\r
+            position[idx-1] := msg_buff[idx];\r
+            idx++;\r
+        until (msg_buff[idx]==MSG_TERM)\r
+        draw_position();\r
+    endif\r
+    if((msg_buff[0]==MSG_FSM_STATE)&&(isStatus==1))\r
+        idx:=1;\r
+        repeat\r
+            fsm_state[idx-1] := msg_buff[idx];\r
+            idx++;\r
+        until(msg_buff[idx]==MSG_TERM)\r
+        fsm_state_len := idx-1;\r
+        update_fsm_state();\r
+    endif\r
+    if((msg_buff[0]==MSG_COLOR)&&(isStatus==1))\r
+        if(msg_buff[1]==1)\r
+            game_color := RED;\r
+        else\r
+            game_color := BLUE;\r
+        endif\r
+    endif\r
+    return 0;\r
+endfunc\r
+\r
+func get_msg()\r
+    var in, idx, bad;\r
 \r
+    idx:=0;\r
+    bad:=0;\r
+\r
+    repeat\r
+        in:=serin();\r
+        if(in!=-1)\r
+            msg_buff[idx] := in;\r
+            idx++;\r
+        else\r
+            break;\r
+        endif\r
+    until((in==MSG_TERM)||(idx==MSG_BUFF_LEN))\r
+    msg_len := idx;\r
+\r
+    process_msg();\r
+    return 0;\r
+endfunc\r
 \r
 func bt_pressed_s_row1(var x)\r
     if(x<bt_x[BT_S_RBRUSH_X])\r
@@ -564,7 +727,11 @@ func bt_pressed_start()
 endfunc\r
 \r
 func bt_pressed_status()\r
-    bt_status(DOWN, GREEN);\r
+    bt_status(DOWN, GREEN, TXT_BT_STATUS);\r
+endfunc\r
+\r
+func bt_pressed_control()\r
+    bt_status(DOWN, GREEN, TXT_BT_CONTROL);\r
 endfunc\r
 \r
 func bt_unpressed_s_row1(var x)\r
@@ -606,10 +773,15 @@ func bt_unpressed_start()
 endfunc\r
 \r
 func bt_unpressed_status()\r
-    bt_status(UP, GRAY);\r
+    bt_status(UP, GRAY, TXT_BT_STATUS);\r
     switch_to_status();\r
 endfunc\r
 \r
+func bt_unpressed_control()\r
+    bt_status(UP, GRAY, TXT_BT_CONTROL);\r
+    switch_to_control();\r
+endfunc\r
+\r
 func touchscreen() \r
     var col, state, x, y;\r
     state := touch_Get(TOUCH_STATUS);               // get touchscreen status\r
@@ -622,6 +794,7 @@ func touchscreen()
             x := touch_Get(TOUCH_GETX);                          \r
             y := touch_Get(TOUCH_GETY);\r
             \r
+            if(isControl)\r
             if((y > bt_y[BT_S_ROW1_Y])&&(y<bt_y[BT_S_ROW2_Y]))\r
                 bt_pressed_s_row1(x);                \r
             endif            \r
@@ -649,11 +822,23 @@ func touchscreen()
             if(y > bt_y[BT_STATUS_Y])\r
                 bt_pressed_status();\r
             endif\r
+            \r
+            endif //isControl\r
+            \r
+            if(isStatus)\r
 \r
+                if(y > bt_y[BT_STATUS_Y])\r
+                    bt_pressed_control();\r
+                endif\r
+                \r
+            endif //isStatus\r
         endif\r
         \r
         //-----------------------------------------------------------------------------------------\r
         if(state == TOUCH_RELEASE)                      // if there's a release\r
+\r
+            if(isControl)\r
+\r
             if((y > bt_y[BT_S_ROW1_Y])&&(y<bt_y[BT_S_ROW2_Y]))\r
                 bt_unpressed_s_row1(x);                \r
             endif            \r
@@ -682,6 +867,16 @@ func touchscreen()
                 bt_unpressed_status();\r
             endif\r
 \r
+            endif //isControl\r
+\r
+            if(isStatus==1)\r
+            \r
+            if(y > bt_y[BT_STATUS_Y])\r
+                bt_unpressed_control();\r
+            endif\r
+            \r
+            endif //isStatus\r
+\r
         endif\r
     \r
 endfunc\r
@@ -691,15 +886,17 @@ func main()
     setbaud(BAUD_2400); \r
     touch_Set(TOUCH_ENABLE);\r
     touch_Set(2); \r
-    draw_ctrl_titles();\r
-    draw_servos_buttons();\r
-    draw_drives_buttons();\r
-    draw_robot_control_buttons();\r
-    draw_status_button();\r
+    isStatus := 1;\r
+    isControl := 0;\r
+    draw_status();\r
+//    draw_control();\r
+\r
     repeat\r
         touchscreen();\r
         ch:=serin();\r
         if(ch==MSG_START)\r
+            get_msg();\r
+            \r
         endif\r
     forever\r
 endfunc\r
@@ -711,3 +908,8 @@ endfunc
 \r
 \r
 \r
+\r
+\r
+\r
+\r
+\r

diff --git a/src/robofsm/eb2008/fsmdisplay.cc b/src/robofsm/eb2008/fsmdisplay.cc
index e0d1f4097856e4e192f5b4e78f3fe7406dd5930c..e3a07a4f784bbf9d93237c7abee0b9b62778f07f 100644 (file)
--- a/src/robofsm/eb2008/fsmdisplay.cc
+++ b/src/robofsm/eb2008/fsmdisplay.cc
@@ -29,6 +29,8 @@
 struct sercom_data *sercom;
 uint8_t msg[10];
 bool msg_waiting;
+bool wait_flag;
+uint8_t mode_to_go;
 
 void serial_comm(int status)
 {
@@ -122,26 +124,30 @@ int set_actuators(uint8_t actuator)
 }
 
 int process_msg(uint8_t *buff) {
+       printf("buff[1]=%d\n",buff[1]);
        switch(buff[1]) {
-               case CHANGE_MODE_REQ:
-                       if(buff[2]==MODE_CONS) {
-                               uoled_switch_mode_rep(MODE_CONS, CHANGE_MODE_OK);
-                               return MODE_CONS;
-                       }
-                       if(buff[2]==MODE_STATUS) {
-                               uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_OK);
-                               return MODE_STATUS;
-                       }
-                       if(buff[2]==MODE_MAP) {
-                               uoled_switch_mode_rep(MODE_MAP, CHANGE_MODE_OK);
-                               return MODE_MAP;
-                       }
-                       if(buff[2]==MODE_MISC) {
-                               uoled_switch_mode_rep(MODE_MISC, CHANGE_MODE_OK);
-                               return MODE_MISC;
-                       }
-                       break;
-               case MSG_ACTUATORS:
+               case SWITCH_TO_STATUS:
+                       uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
+                       wait_flag = true;
+                       printf("changing mode to status\n");
+                       return MODE_STATUS;
+                       break;
+               case SWITCH_TO_CONTROL:
+                       uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_CONTROL);
+                       wait_flag = true;
+                       printf("changing mode to control\n");
+                       return MODE_CONTROL;
+                       break;
+               case SWITCH_TO_STATUS_DONE:
+                       wait_flag = false;
+                       mode_to_go = MODE_STATUS;
+                       break;
+               case SWITCH_TO_CONTROL_DONE:
+                       wait_flag = false;
+                       printf("wait flag = false\n");
+                       mode_to_go = MODE_CONTROL;
+                       break;
+               case ACTUATORS:
                        set_actuators(buff[2]);
                        break;
                default:
@@ -152,63 +158,20 @@ int process_msg(uint8_t *buff) {
 
 
 FSM_STATE_DECL(control);
-FSM_STATE_DECL(console);
 FSM_STATE_DECL(status);
-FSM_STATE_DECL(map);
-FSM_STATE_DECL(misc);
-FSM_STATE_DECL(pokus);
+FSM_STATE_DECL(wait_for_mode_switch);
 
 FSM_STATE(disp_init) {
 
        if (FSM_EVENT == EV_ENTRY) {
                msg_waiting = false;
                init_display();
-               FSM_TRANSITION(control);
+//             FSM_TRANSITION(control);
+               FSM_TRANSITION(status);
        }
 }
 
 
-FSM_STATE(console)
-{
-       uint8_t buff[10];
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       FSM_TIMER(1000);
-                       break;
-               case EV_SWITCH_TO_CONS:
-                       break;
-               case EV_SWITCH_TO_STATUS:
-                       break;
-               case EV_SWITCH_TO_MAP:
-                       break;
-               case EV_SWITCH_TO_MISC:
-                       break;
-               case EV_TIMER:
-                       ROBOT_LOCK(disp);
-                       if(msg_waiting) {
-                               memcpy(buff, msg, 10);
-                               msg_waiting = false;
-                               ROBOT_UNLOCK(disp);
-                               switch(process_msg(buff)) {
-                                       case MODE_STATUS:
-                                               FSM_TRANSITION(status);
-                                               break;
-                                       case MODE_MAP:
-                                               FSM_TRANSITION(map);
-                                               break;
-                                       case MODE_MISC:
-                                               FSM_TRANSITION(misc);
-                                               break;
-                               }
-                       }
-                       else
-                               ROBOT_UNLOCK(disp);
-                       FSM_TIMER(1000);
-                       break;
-               default:
-                       break;
-       }
-}
 
 FSM_STATE(status)
 {
@@ -228,14 +191,11 @@ FSM_STATE(status)
                                msg_waiting = false;
                                ROBOT_UNLOCK(disp);
                                switch(process_msg(buff)) {
-                                       case MODE_CONS:
-                                               FSM_TRANSITION(console);
-                                               break;
-                                       case MODE_MAP:
-                                               FSM_TRANSITION(map);
+                                       case MODE_CONTROL:
+                                               FSM_TRANSITION(wait_for_mode_switch);
                                                break;
-                                       case MODE_MISC:
-                                               FSM_TRANSITION(misc);
+                                       default:
+                                               printf("display: request to switch to unknown mode\n");
                                                break;
                                }
                        }
@@ -244,91 +204,33 @@ FSM_STATE(status)
                        FSM_TIMER(1000);
                        break;
                case EV_SWITCH_TO_CONS:
-                       uoled_switch_mode_rep(MODE_CONS, CHANGE_MODE_OK);
-                       FSM_TRANSITION(console);
                        break;
                case EV_SWITCH_TO_STATUS:
-                       uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_OK);
-                       FSM_TRANSITION(status);
                        break;
                case EV_SWITCH_TO_MAP:
-                       uoled_switch_mode_rep(MODE_MAP, CHANGE_MODE_OK);
-                       FSM_TRANSITION(map);
                        break;
                case EV_SWITCH_TO_MISC:
-                       uoled_switch_mode_rep(MODE_MISC, CHANGE_MODE_OK);
-                       FSM_TRANSITION(misc);
                        break;
                default:
                        break;
        }
 }
 
-
-FSM_STATE(map)
+FSM_STATE(control)
 {
        uint8_t buff[10];
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       FSM_TIMER(1000);
-                       break;
-               default:
-                       break;
-               case EV_SWITCH_TO_CONS:
-                       FSM_TRANSITION(console);
-                       break;
-               case EV_SWITCH_TO_STATUS:
-                       FSM_TRANSITION(status);
-                       break;
-               case EV_SWITCH_TO_MAP:
-                       FSM_TRANSITION(map);
-                       break;
-               case EV_SWITCH_TO_MISC:
-                       FSM_TRANSITION(misc);
-                       break;
-               case EV_TIMER:
-                       ROBOT_LOCK(disp);
-                       if(msg_waiting) {
-                               memcpy(buff, msg, 10);
-                               msg_waiting = false;
-                               ROBOT_UNLOCK(disp);
-                               switch(process_msg(buff)) {
-                                       case MODE_CONS:
-                                               FSM_TRANSITION(console);
-                                               break;
-                                       case MODE_STATUS:
-                                               FSM_TRANSITION(status);
-                                               break;
-                                       case MODE_MISC:
-                                               FSM_TRANSITION(misc);
-                                               break;
-                               }
-                       }
-                       else
-                               ROBOT_UNLOCK(disp);
-                       FSM_TIMER(1000);
-                       break;
-       }
-}
 
-FSM_STATE(misc)
-{
-       uint8_t buff[10];
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        FSM_TIMER(1000);
                        break;
                case EV_SWITCH_TO_CONS:
-                       FSM_TRANSITION(console);
                        break;
                case EV_SWITCH_TO_STATUS:
-                       FSM_TRANSITION(status);
                        break;
                case EV_SWITCH_TO_MAP:
-                       FSM_TRANSITION(map);
                        break;
                case EV_SWITCH_TO_MISC:
-                       FSM_TRANSITION(misc);
                        break;
                case EV_TIMER:
                        ROBOT_LOCK(disp);
@@ -337,14 +239,11 @@ FSM_STATE(misc)
                                msg_waiting = false;
                                ROBOT_UNLOCK(disp);
                                switch(process_msg(buff)) {
-                                       case MODE_CONS:
-                                               FSM_TRANSITION(console);
-                                               break;
-                                       case MODE_MAP:
-                                               FSM_TRANSITION(map);
-                                               break;
                                        case MODE_STATUS:
-                                               FSM_TRANSITION(status);
+                                               FSM_TRANSITION(wait_for_mode_switch);
+                                               break;
+                                       default:
+                                               printf("display: request to switch to unknown mode\n");
                                                break;
                                }
                        }
@@ -356,14 +255,15 @@ FSM_STATE(misc)
                        break;
        }
 }
+       
 
-FSM_STATE(control)
+FSM_STATE(wait_for_mode_switch)
 {
        uint8_t buff[10];
 
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       FSM_TIMER(1000);
+                       FSM_TIMER(500);
                        break;
                case EV_SWITCH_TO_CONS:
                        break;
@@ -379,25 +279,22 @@ FSM_STATE(control)
                                memcpy(buff, msg, 10);
                                msg_waiting = false;
                                ROBOT_UNLOCK(disp);
-                               switch(process_msg(buff)) {
-                                       case MODE_STATUS:
-                                               FSM_TRANSITION(status);
-                                               break;
-                                       case MODE_MAP:
-                                               FSM_TRANSITION(map);
-                                               break;
-                                       case MODE_MISC:
-                                               FSM_TRANSITION(misc);
-                                               break;
-                               }
+                               process_msg(buff);
                        }
                        else
                                ROBOT_UNLOCK(disp);
-                       FSM_TIMER(1000);
+                       if(wait_flag) 
+                               break;
+                       else {
+                               printf("wait flag cleared\n");
+                               if(mode_to_go==MODE_STATUS)
+                                       FSM_TRANSITION(status);
+                               else if(mode_to_go==MODE_CONTROL)
+                                       FSM_TRANSITION(control);
+                       }
                        break;
                default:
                        break;
        }
 }
-       
 

diff --git a/src/robofsm/test/display.cc b/src/robofsm/test/display.cc
index d59dc39f272a3f4de0222a165e10268ed01c580d..fbb4c974da705acac0c8a55de43ec508d4e23a75 100644 (file)
--- a/src/robofsm/test/display.cc
+++ b/src/robofsm/test/display.cc
@@ -18,7 +18,7 @@ void callback(struct robo_fsm *fsm)
 {
        if(fsm->last_state_name!=NULL) {        
                printf("State changed from %s to %s\n", fsm->last_state_name, fsm->state_name);
-               uoled_send_state(fsm->state_name, strlen(fsm->state_name));
+//             uoled_send_state(fsm->state_name, strlen(fsm->state_name));
 //             printf("last_state strlen=%d\n",strlen(fsm->last_state_name));
        }
 }

diff --git a/src/sercom/sercom.c b/src/sercom/sercom.c
index ac16711c69fe7dfe4ff1e1987ff0735828bac5da..ec13bd1c1457f00003835b3609fef71c15dc011b 100644 (file)
--- a/src/sercom/sercom.c
+++ b/src/sercom/sercom.c
@@ -25,7 +25,6 @@ unsigned char sercom_wait_flag = 1;
 
 void sercom_sighandler(int status)
 {
-       printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!received SIGIO signal.\n");
        sercom_wait_flag = 0;
 }
 
@@ -206,14 +205,17 @@ void sercom_open(struct sercom_data *sercom)
        tcgetattr(sercom->fd, &sercom->oldtio);
        memset(&sercom->newtio, 0, sizeof(sercom->newtio));
 
-       sercom->newtio.c_cflag = sercom_get_baudrate(sercom->baudrate) |
-                       sercom_get_databits(sercom->databits) |
-                       sercom_get_stopbits(sercom->stopbits) |
-                       sercom_get_parity(sercom->parity) |
-                       ~CRTSCTS | CLOCAL | CREAD;
-       sercom->newtio.c_iflag &= ~IGNPAR | ICRNL | ~IXON | ~IXOFF;
-       sercom->newtio.c_oflag &= 0;
-       sercom->newtio.c_lflag &= ~ICANON | ~ECHO | ~ECHOE | ~ECHOK | ~FLUSHO;
+//     sercom->newtio.c_cflag = sercom_get_databits(sercom->databits) |
+//                     sercom_get_stopbits(sercom->stopbits) |
+//                     sercom_get_parity(sercom->parity) |
+//                     ~CRTSCTS | CLOCAL | CREAD;
+       sercom->newtio.c_cflag |= CS8 | CLOCAL | CREAD | HUPCL;
+       sercom->newtio.c_cflag &= ~PARENB;
+       sercom->newtio.c_iflag |= ICRNL | IGNBRK;
+       sercom->newtio.c_iflag &= ~IGNPAR | ~IXON | ~IXOFF;
+       sercom->newtio.c_oflag = 0;
+       sercom->newtio.c_lflag |= ICANON;// | ECHO | ECHOE;
+//     sercom->newtio.c_lflag &= ~(ECHO | ECHOE | ECHOK | FLUSHO);
        cfsetispeed(&sercom->newtio, sercom_get_baudrate(sercom->baudrate));
        cfsetospeed(&sercom->newtio, sercom_get_baudrate(sercom->baudrate));
        /* blocking read until one character arrives */

diff --git a/src/ulcdd/oledlib.h b/src/ulcdd/oledlib.h
index 2a98e96f6c6fc7ab224f2ed603b23d3e4575dcec..48cbd95a8d4786454bc300ec67be5ab477b8862f 100644 (file)
--- a/src/ulcdd/oledlib.h
+++ b/src/ulcdd/oledlib.h
@@ -19,16 +19,21 @@
 #define COLOR_MSG 164
 #define POSITION_MSG 165
 
-#define MSG_ACTUATORS 180
+#define ACTUATORS 180
+#define GET_READY 181
+#define ROBOT_START 182
+#define SWITCH_TO_STATUS 183
+#define SWITCH_TO_CONTROL 184
+#define SWITCH_TO_STATUS_DONE 185
+#define SWITCH_TO_CONTROL_DONE 186
 
 #define CHANGE_MODE_REP 200
 #define CHANGE_MODE_REQ 201
-#define CHANGE_MODE_OK 202
+#define CHANGE_MODE_STATUS 202
+#define CHANGE_MODE_CONTROL 203
 
-#define MODE_CONS 0
 #define MODE_STATUS 1
-#define MODE_MAP 2
-#define MODE_MISC 3
+#define MODE_CONTROL 2
 
 
 #define LBRUSH_SERVO 1