robofsm: Fixed initialization and added possibility to deposit before competition

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index e40ecbf927db8887710f7afec55d0249c20f051f..87b489cc5295587d4cc66d3d14174095e81ba9fc 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -259,6 +259,8 @@ 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 */
@@ -281,7 +283,7 @@ FSM_STATE(init)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       FSM_TRANSITION(wait_for_start);
+                       FSM_TRANSITION(wait_for_laser);
                        break;
                default:
                        break;
@@ -290,9 +292,12 @@ FSM_STATE(init)
 
 FSM_STATE(wait_for_laser) 
 {
-       /* start event ocurred */
        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));
@@ -301,23 +306,47 @@ FSM_STATE(wait_for_laser)
                        brushes_out();
                        open_bottom_door();
                        close_back_door();
-                       FSM_TIMER(2000);
-                       break;
-               case EV_START_LASER:
                        robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
                                                PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
                                                DEG2RAD(-45));
+
+                       robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
+                       ORTEPublicationSend(robot.orte.publication_laser_cmd);
                        robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
                        ORTEPublicationSend(robot.orte.publication_laser_cmd);
                        robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
                        ORTEPublicationSend(robot.orte.publication_laser_cmd);
+
+#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(go_to_container);
-//                     FSM_TRANSITION(get_balls);
+                       FSM_TRANSITION(wait_for_start);
+                       break;
+               default:
                        break;
-               default: break;
        }
 }
 
@@ -524,11 +553,14 @@ FSM_STATE(go_back_to_edge)
                case EV_MOTION_DONE:
                        if (closed_to_container()) {
                                robot.carousel_pos = 0;
-                               FSM_TRANSITION(deposite_balls);
+                               SUBFSM_TRANSITION(deposite_balls, NULL);
                        } else {
                                DBG("FIXME: go_closer_to_container\n");
                        }
                        break;
+               case EV_RETURN:
+                       FSM_TRANSITION(decide_where_now);
+                       break;
                default: break;
        }
 }
@@ -648,7 +680,7 @@ FSM_STATE(deposite_balls)
                                robot.carousel_cnt = 0;
                                robot.carousel_pos = 0;
                                DBG("carousel_cnt = %d\n", robot.carousel_cnt);
-                               FSM_TRANSITION(decide_where_now);
+                               SUBFSM_RET(NULL);
                        }
                        break;
                case EV_RETURN:

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index dea456c94bbe7d62d35577a4daeda64947c92902..c21a6b1b8a69bdb5b502f58ac9ae90859262a610 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -181,21 +181,28 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
        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_LASER, NULL);
+                                               if (!instance->start) {
+                                                       FSM_SIGNAL(MAIN, EV_START, NULL);
                                                        state = LASER_STARTED;
                                                }
                                                break;
                                        case LASER_STARTED:
-                                               if (!instance->start) {
+                                               if (instance->start) {
                                                        FSM_SIGNAL(MAIN, EV_START, NULL);
                                                        state = COMPETITION_STARTED;
                                                }
                                                break;
                                        case COMPETITION_STARTED:
-                                               if (instance->start) {
+                                               if (!instance->start) {
                                                        robot_exit();
                                                }
                                                break;