fsmdisplay: added initial mode setting

diff --git a/src/robofsm/eb2008/fsmdisplay.cc b/src/robofsm/eb2008/fsmdisplay.cc
index 56b372b5e97e7e70f97505f334d3b2d56626ba85..b95255832a2786e73797075bd9cab77847bea1a6 100644 (file)
--- a/src/robofsm/eb2008/fsmdisplay.cc
+++ b/src/robofsm/eb2008/fsmdisplay.cc
@@ -201,21 +201,36 @@ int process_msg(uint8_t *buff) {
 }
 
 
+FSM_STATE_DECL(disp_init);
 FSM_STATE_DECL(control);
 FSM_STATE_DECL(status);
+FSM_STATE_DECL(set_init_mode);
 FSM_STATE_DECL(wait_for_mode_switch);
 
-FSM_STATE(disp_init) {
+FSM_STATE(disp_init) 
+{
 
        if (FSM_EVENT == EV_ENTRY) {
                msg_waiting = false;
                init_display();
 //             FSM_TRANSITION(control);
-               FSM_TRANSITION(status);
+               FSM_TRANSITION(set_init_mode);
        }
 }
 
+FSM_STATE(set_init_mode)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       uoled_switch_mode_rep(MODE_CONTROL, CHANGE_MODE_CONTROL);
+                       FSM_TIMER(500);
+                       break;
+               case EV_TIMER:
+                       FSM_TRANSITION(control);
+                       break;
 
+       }
+}
 
 FSM_STATE(status)
 {