robofsm: fix the display to work correctly in combination with the plug.

diff --git a/src/robofsm/eb2008/fsmdisplay.cc b/src/robofsm/eb2008/fsmdisplay.cc
index 1d472c2c92c1982c284e9facaa27c12bed0b9f3d..a9aac2e34ccea6416c432177d621169d89c6d953 100644 (file)
--- a/src/robofsm/eb2008/fsmdisplay.cc
+++ b/src/robofsm/eb2008/fsmdisplay.cc
@@ -105,8 +105,14 @@ int set_actuators(uint8_t actuator)
                case RBRUSH_DRIVE:
                        if (robot.orte.laser_cmd.speed == LASER_DRIVE_ON) {
                                off_laser();
+                               ROBOT_LOCK(state);
+                               robot.state = JUST_STARTED;
+                               ROBOT_UNLOCK(state);
                        } else {
                                on_laser();
+                               ROBOT_LOCK(state);
+                               robot.state = LASER_STARTED;
+                               ROBOT_UNLOCK(state);
                        }
                        break;
                case BAGR_DRIVE:

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index 28e283144ac86761b872d538939199af70b23bfd..5b5b8ceaa97a4e5c6918eefac1104e08282bd2d7 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -259,8 +259,6 @@ void move_trans_callback(struct robo_fsm *fsm)
 
 /* initial and starting states */
 FSM_STATE_DECL(init);
-FSM_STATE_DECL(wait_for_deposit);
-FSM_STATE_DECL(wait_for_laser);
 FSM_STATE_DECL(wait_for_start);
 FSM_STATE_DECL(decide_where_now);
 /* movement states */
@@ -283,64 +281,11 @@ FSM_STATE(init)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
-                                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-                                               DEG2RAD(-45));
-                       FSM_TRANSITION(wait_for_start);
-                       break;
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(wait_for_laser) 
-{
-       switch (FSM_EVENT) {
-#ifdef WAIT_FOR_START
-               case EV_START:
-#else
-               case EV_ENTRY:
-#endif         
-                       robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
-                                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-                                               DEG2RAD(-45));
                        off_brush_left();
                        off_brush_right();
                        brushes_out();
                        open_bottom_door();
                        close_back_door();
-                       robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
-                                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-                                               DEG2RAD(-45));
-
-                       on_laser();
-
-#ifdef WAIT_FOR_START                  
-                       FSM_TRANSITION(wait_for_deposit);
-#else
-                       FSM_TRANSITION(wait_for_start);
-#endif
-               default:
-                       break;
-       }
-}
-
-
-FSM_STATE(wait_for_deposit) 
-{
-       /* start event ocurred */
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       FSM_TIMER(2000);
-                       break;
-               case EV_START:
-                       // If start is given in two seconds
-                       SUBFSM_TRANSITION(deposite_balls, NULL);
-                       break;
-               case EV_RETURN:
-                       FSM_TRANSITION(wait_for_deposit);
-                       break;
-               case EV_TIMER:
                        FSM_TRANSITION(wait_for_start);
                        break;
                default:
@@ -364,11 +309,22 @@ FSM_STATE(wait_for_start)
 #endif
                        on_laser();
 
+                       robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
+                                               PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
+                                               DEG2RAD(-45));
                        /* start to do something */
+                       ROBOT_LOCK(state);
+                       robot.state = COMPETITION_STARTED;
+                       ROBOT_UNLOCK(state);
                        FSM_TRANSITION(decide_where_now);
                        break;
+               case EV_LASER_POWER:
+                       on_laser();
+                       break;
                case EV_DEPOSITE_BALLS:
-                       FSM_TRANSITION(wait_for_deposit);
+                       SUBFSM_TRANSITION(deposite_balls, NULL);
+                       break;
+               case EV_RETURN:
                        break;
                default: break;
        }

diff --git a/src/robofsm/eb2008/robot_eb2008.c b/src/robofsm/eb2008/robot_eb2008.c
index 0e108376c3a9bee71ac950c60e5e35e817c67c8f..ad94fa3ef641306e331d2d05682967fd3407d96f 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.c
+++ b/src/robofsm/eb2008/robot_eb2008.c
@@ -173,6 +173,7 @@ int robot_init()
 
        robot.obstacle_avoidance_enabled = false;
        robot.laser_enabled = true;
+       robot.state = JUST_STARTED;
 
        /* init ORTE domain, create publishers, subscribers, .. */
        robot_init_orte();

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index 0ba47a99684256f59d0ea31f102713c2fe835fd5..c3eead6cc5b9d1cae8355161936ba632f2f17920 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -47,6 +47,12 @@ enum ball_color {
        WHITE_BALL
 };
 
+enum robot_state {
+       JUST_STARTED = 0,
+       LASER_STARTED,
+       COMPETITION_STARTED
+};
+
 /**
  * FSM
  */
@@ -122,6 +128,7 @@ extern struct lock_log robot_lock_log;
 #define __robot_lock_bumper            lock
 #define __robot_lock_drives            lock
 #define __robot_lock_disp              lock_disp
+#define __robot_lock_state             lock
 
 /* robot's main data structure */
 struct robot_eb2008 {
@@ -144,6 +151,8 @@ struct robot_eb2008 {
        unsigned char carousel_cnt; /* The number of balls in carousel */
        unsigned char carousel_pos; /* required position of the carousel */
 
+       enum robot_state state;
+
        /** Temporary storage for new trajectory. After the trajectory creation is
         * finished, this trajectory is submitted to fsmmove. */
        void *new_trajectory;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index c21a6b1b8a69bdb5b502f58ac9ae90859262a610..7053bdb37753fe5e2398900285c9f89c5c272e02 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -172,43 +172,44 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 {
        struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
        static struct robot_cmd_type last_instance;
-       static enum {
-               JUST_STARTED,
-               LASER_STARTED,
-               COMPETITION_STARTED,
-       } state = JUST_STARTED;
+       enum robot_state state;
 
        switch (info->status) {
                case NEW_DATA:
-                       if (instance->start != last_instance.start) {
-                               printf("state %d ", state);
-                               if (instance->start)
-                                       printf("plugged in\n");
-                               else
-                                       printf("plugged out\n");
-
-                               
-                               switch (state) {
-                                       case JUST_STARTED:
-                                               if (!instance->start) {
-                                                       FSM_SIGNAL(MAIN, EV_START, NULL);
-                                                       state = LASER_STARTED;
-                                               }
-                                               break;
-                                       case LASER_STARTED:
-                                               if (instance->start) {
-                                                       FSM_SIGNAL(MAIN, EV_START, NULL);
-                                                       state = COMPETITION_STARTED;
-                                               }
+                       if (instance->start == last_instance.start)
+                               break;
+
+                       ROBOT_LOCK(state);
+                       state = robot.state;
+                       ROBOT_UNLOCK(state);
+                       printf("in state=%d \n", state);
+
+                       switch (state) {
+                               case JUST_STARTED:
+                                       if (instance->start)
                                                break;
-                                       case COMPETITION_STARTED:
-                                               if (!instance->start) {
-                                                       robot_exit();
-                                               }
+                                       FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
+                                       state = LASER_STARTED;
+                                       break;
+
+                               case LASER_STARTED:
+                                       if (!instance->start)
                                                break;
-                               }
+                                       FSM_SIGNAL(MAIN, EV_START, NULL);
+                                       state = COMPETITION_STARTED;
+                                       break;
+
+                               case COMPETITION_STARTED:
+                                       if (!instance->start) {
+                                               robot_exit();
+                                       }
+                                       break;
                        }
                        last_instance = *instance;
+                       printf("out state=%d \n", state);
+                       ROBOT_LOCK(state);
+                       robot.state = state;
+                       ROBOT_UNLOCK(state);
                        break;
                case DEADLINE:
                        DBG("ORTE deadline occurred - pwr_ctrl receive\n");