robofsm: ball pick up and deposition updated, some part of the homologation program

have been rewritten. Homologation program almost done, need to be tested.

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index f716f514d824dfc04365298c55e2b7cc0fab505d..c7fe5d776bca86a3aab96b0fa1f936b24c4c6ea2 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -31,16 +31,16 @@
 #ifdef COMPETITION
 #define WAIT_FOR_START
 #define COMPETITION_TIME_DEFAULT       90
-#define WAIT_FOR_DEPOSITION_DEFAULT    60
+#define TIME_TO_DEPOSITE_DEFAULT       60
 #else
 #undef WAIT_FOR_START
 #define COMPETITION_TIME_DEFAULT       900
-#define WAIT_FOR_DEPOSITION_DEFAULT    60
+#define TIME_TO_DEPOSITE_DEFAULT       60
 #endif
 
 /* competition time in seconds */
 #define COMPETITION_TIME       COMPETITION_TIME_DEFAULT
-#define WAIT_FOR_DEPOSITION    WAIT_FOR_DEPOSITION_DEFAULT
+#define TIME_TO_DEPOSITE       TIME_TO_DEPOSITE_DEFAULT
 /* competition time in seconds */
 
 enum {
@@ -49,6 +49,10 @@ enum {
        CENTER
 };
 
+/************************************************************************
+ * MISC FUNCTIONS
+ ************************************************************************/
+
 /**
  * Convert back sharps' measured values to mm.
  *
@@ -193,7 +197,7 @@ void *wait_for_end(void *arg)
  */
 void *wait_to_deposition(void *arg)
 {
-       sleep(WAIT_FOR_DEPOSITION);
+       sleep(TIME_TO_DEPOSITE);
        FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
        return NULL;
 }
@@ -230,22 +234,28 @@ void robot_go_backward_to_point(struct ref_pos_type des_pos)
        robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
 }
 
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+/* initial and starting states */
 FSM_STATE_DECL(init);
+FSM_STATE_DECL(wait_for_start);
+/* movement states */
 FSM_STATE_DECL(go_to_our_white_dispenser);
 FSM_STATE_DECL(go_to_our_white_dispenser2);
+FSM_STATE_DECL(go_far_from_dispenser);
+FSM_STATE_DECL(go_from_our_dispenser);
+FSM_STATE_DECL(go_to_container);
+/* ball and carousel states */
 FSM_STATE_DECL(get_balls);
 FSM_STATE_DECL(next_carousel_position);
-FSM_STATE_DECL(wait_for_start);
-FSM_STATE_DECL(go_to_container);
 FSM_STATE_DECL(deposite_balls);
-FSM_STATE_DECL(go_far_from_dispenser);
-FSM_STATE_DECL(go_from_our_dispenser);
 
-/**
- * Set starting position, playground`s safety zone and other obstacles.
- * 
- * @ingroup fsmmain
- */
+/************************************************************************
+ * INITIAL AND STARTING STATES
+ ************************************************************************/
+
 FSM_STATE(init) 
 {
        /* start event ocurred */
@@ -262,7 +272,10 @@ FSM_STATE(init)
                        close_back_door();
 //                     FSM_TRANSITION(wait_for_start);
 //                     FSM_TRANSITION(go_to_container);
-                       FSM_TRANSITION(get_balls);
+//                     FSM_TRANSITION(get_balls);
+                       robot.carousel_cnt = 5;
+                       robot.carousel_pos = 0;
+                       FSM_TRANSITION(deposite_balls);
                        break;
                default: break;
        }
@@ -290,6 +303,10 @@ FSM_STATE(wait_for_start)
        }
 }
 
+/************************************************************************
+ * MOVEMENT STATES
+ ************************************************************************/
+
 FSM_STATE(go_to_our_white_dispenser)
 {
        switch (FSM_EVENT) {
@@ -356,6 +373,43 @@ FSM_STATE(go_from_our_dispenser)
        }
 }
 
+#define GO_TO_CONTAINER_TIMER  10000
+
+FSM_STATE(go_to_container)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       off_brush_left();
+                       off_brush_right();
+                       off_bagr();
+                       robot_trajectory_new_backward(NULL);
+                       robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.6, 1.0);                                        
+                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.4, 
+                                               0, NO_TURN());
+                       FSM_TIMER(GO_TO_CONTAINER_TIMER);
+                       break;
+               case EV_TIMER:
+               case EV_MOTION_DONE:
+                       if (closed_to_container()) {
+                               robot.carousel_pos = 0;
+                               FSM_TRANSITION(deposite_balls);
+                       } else {
+                               DBG("FIXME: go_closer_to_container\n");
+                       }
+                       break;
+               default: break;
+       }
+}
+
+/************************************************************************
+ * BALLS AND CAROUSEL MANIPULATION STATES
+ ************************************************************************/
+
+#define MAX_GET_BALL_ATTEMPS   5
+#define GET_BALL_TIMER         1500
+#define WAIT_BALL_INSIDE       5000
+#define GET_BALL_BAGR_SPEED    120
+
 FSM_STATE(get_balls)
 {
        static int get_ball_attemps = 0;
@@ -363,8 +417,9 @@ FSM_STATE(get_balls)
 
        switch (FSM_EVENT) {
                case EV_ENTRY:
+               case EV_RETURN:
                        open_bottom_door();
-                       set_bagr(120);
+                       set_bagr(GET_BALL_BAGR_SPEED);
                        brushes_drives_in();
                        FSM_TIMER(500);
                        break;
@@ -373,146 +428,104 @@ FSM_STATE(get_balls)
                                ball_inside = 0;
                                close_bottom_door();
                                off_bagr();
-                               if (robot.carousel_cnt >= CAROUSEL_SIZE) {
+                               if (robot.carousel_cnt >= BALL_TO_COLLECT) {
+                                       /* FIXME: just to test deposition */
+                                       robot.carousel_pos = 0;
+                                       FSM_TRANSITION(deposite_balls);
 //                                     printf("go_to_container\n");
 //                                     FSM_TRANSITION(go_to_container);
-                                       FSM_TRANSITION(deposite_balls);
                                } else {
-                                       FSM_TRANSITION(next_carousel_position);
+                                       robot.carousel_pos = 
+                                               (robot.carousel_pos+2) % CAROUSEL_SIZE;
+                                       SUBFSM_TRANSITION(
+                                               next_carousel_position, NULL);
                                }
-                       } else if (get_ball_attemps++ < 5) {
-                               FSM_TIMER(1500);
+                       } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) {
+                               FSM_TIMER(GET_BALL_TIMER);
                        } else {
-                               printf("go_to_container\n");
+                               /* FIXME: */
+                               DBG("go_to_container\n");
 //                             FSM_TRANSITION(go_to_container);
                        }
                        break;
                case EV_BALL_INSIDE:
+                       /* ball is already inside */
                        if (ball_inside)
                                break;
 
                        ball_inside = 1;
-                       robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] = (enum ball_color)robot.cmu.color;
+                       robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] = 
+                               (enum ball_color)robot.cmu.color;
                        robot.carousel_cnt++;
-                       printf("cnt=%d\n", robot.carousel_cnt);
-                       FSM_TIMER(5000);
+                       DBG("ball cnt=%d\n", robot.carousel_cnt);
+                       FSM_TIMER(WAIT_BALL_INSIDE);
                        break;
                default: break;
        }
 }
 
+#define MAX_CAROUSEL_ATTEMPTS  3
+#define CAROUSEL_ATTEMPT_TIMER 1000
+
 FSM_STATE(next_carousel_position)
 {
-       static int carousel_attempts = 0;
+       static int carousel_attempts;
        int rv;
 
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       robot.carousel_pos = 
-                               (robot.carousel_pos+2) % CAROUSEL_SIZE;
+                       carousel_attempts = 0;
                        set_carousel(robot.carousel_pos);
-                       FSM_TIMER(1000);
+                       FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
                        break;
                case EV_TIMER:
                        carousel_attempts++;
+                       /* check out, if the carousel reached position */
                        ROBOT_LOCK(carousel);
                        rv = (robot.drives.carousel_pos == robot.carousel_pos);
                        ROBOT_UNLOCK(carousel);
-                       if (rv) {
-                               FSM_TRANSITION(get_balls);
-                       } else if (carousel_attempts < 3) {
-                               FSM_TIMER(1000);
+                       if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) {
+                               FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
+                       } else if(rv) {
+                               DBG("carousel reached the position.\n");
+                               SUBFSM_RET(NULL);
                        } else {
-                               printf("go_to_container\n");
-//                             FSM_TRANSITION(go_to_container);
+                               DBG("FIXME: carousel lost.\n");
+                               SUBFSM_RET(NULL);
                        }
                        break;
                default: break;
        }
 }
 
-FSM_STATE(next_carousel_position2)
-{
-       static int carousel_attempts = 0;
-       int rv;
-
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       set_carousel(robot.carousel_pos);
-                       break;
-               case EV_TIMER:
-                       carousel_attempts++;
-                       ROBOT_LOCK(carousel);
-                       rv = (robot.drives.carousel_pos == robot.carousel_pos);
-                       ROBOT_UNLOCK(carousel);
-                       if (rv) {
-                               FSM_TRANSITION(deposite_balls);
-                       } else if (carousel_attempts < 3) {
-                               FSM_TIMER(500);
-                       } else {
-                               FSM_TRANSITION(deposite_balls);
-                       }
-                       break;
-               default: break;
-       }
-}
+#define WAIT_FOR_DEPOSITION_TIMER      1500
 
-FSM_STATE(go_to_container)
+FSM_STATE(deposite_balls)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       off_brush_left();
-                       off_brush_right();
-                       off_bagr();
-                       robot_trajectory_new_backward(NULL);
-                       robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.6, 1.0);                                        
-                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.4, 
-                                               0, NO_TURN());
-                       FSM_TIMER(10000);
-                       break;
-               case EV_TIMER:
-               case EV_MOTION_DONE:
-                       if (closed_to_container()) {
-                               FSM_TRANSITION(deposite_balls);
+                       if (robot.carousel_pos < CAROUSEL_SIZE) {
+                               close_back_door();
+                               robot.carousel_pos++;
+                               robot.carousel_cnt--;
+                               SUBFSM_TRANSITION(next_carousel_position, NULL);
                        } else {
-                               printf("FIXME:\n");
+                               robot.carousel_cnt = 0;
+                               /* FIXME: */
+                               DBG("go_to_our_white_dispenser\n");
+//                             FSM_TRANSITION(go_to_our_white_dispenser);
                        }
                        break;
-               default: break;
-       }
-}
-
-FSM_STATE(deposite_balls)
-{
-       int carousel_pos = 0;
-       int wait_for_deposition = 0;
-
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       set_carousel(carousel_pos++);
-//                     robot.carousel_cnt--;
-//                     if (robot.carousel_cnt <= 0) {
-//                             robot.carousel_cnt = 0;
-//                             FSM_TRANSITION(go_to_our_white_dispenser);
-//                     }
-//                     ROBOT_LOCK(drives);
-//                     robot.carousel_pos = robot.drives.carousel_pos;
-//                     ROBOT_UNLOCK(drives);
-                       FSM_TIMER(500);
+               case EV_RETURN:
+                       DBG("open_back_door\n");
+                       open_back_door();
+                       FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
                        break;
                case EV_TIMER:
-                       if (!wait_for_deposition) {
-                               open_back_door();
-                               wait_for_deposition = 1;
-                       }
-
-                       FSM_TIMER(500);
-                       if (carousel_pos > 4)
-                               printf("ok\n");
-//                             FSM_TRANSITION(go_to_our_white_dispenser);
-//                     FSM_TRANSITION(next_carousel_position2);
                        close_back_door();
-                       set_carousel(carousel_pos++);
+                       robot.carousel_pos++;
+                       DBG("carousel_cnt = %d\n", robot.carousel_cnt);
+                       FSM_TRANSITION(deposite_balls);
                        break;
                default: break;
        }

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index 5e3ad041a503be2879a6710c8441aa3ed9fdccb7..7d8b50928d3f29fc22aa5de05153ba94491afe93 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -188,9 +188,9 @@ void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
 
        switch (info->status) {
                case NEW_DATA:
-                       ROBOT_LOCK(carousel);
+                       ROBOT_LOCK(drives);
                        robot.drives = *instance;
-                       ROBOT_UNLOCK(carousel);
+                       ROBOT_UNLOCK(drives);
                        break;
                case DEADLINE:
                        DBG("ORTE deadline occurred - drives receive\n");