Merge branch 'master' of trandk1@rtime.felk.cvut.cz:/var/git/eurobot

[eurobot/public.git] / src / robofsm / eb2008 / fsmmain.cc

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index b9924b5fe756785130c8f14367b02f27e9b5ff3a..f716f514d824dfc04365298c55e2b7cc0fab505d 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -257,10 +257,12 @@ FSM_STATE(init)
 //                     robot_set_est_pos_trans(0.15, PLAYGROUND_HEIGHT_M - 0.7, DEG2RAD(180));
                        off_brush_left();
                        off_brush_right();
+                       brushes_out();
                        open_bottom_door();
                        close_back_door();
-                       FSM_TRANSITION(wait_for_start);
+//                     FSM_TRANSITION(wait_for_start);
 //                     FSM_TRANSITION(go_to_container);
+                       FSM_TRANSITION(get_balls);
                        break;
                default: break;
        }
@@ -362,36 +364,37 @@ FSM_STATE(get_balls)
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        open_bottom_door();
-                       set_bagr(140);
+                       set_bagr(120);
                        brushes_drives_in();
-//                     set_bagr(140);
-//                     brushes_drives_in();
                        FSM_TIMER(500);
                        break;
                case EV_TIMER:
                        if (ball_inside) {
-                       printf("tady2\n");
                                ball_inside = 0;
                                close_bottom_door();
+                               off_bagr();
                                if (robot.carousel_cnt >= CAROUSEL_SIZE) {
-                                       FSM_TRANSITION(go_to_container);
+//                                     printf("go_to_container\n");
+//                                     FSM_TRANSITION(go_to_container);
+                                       FSM_TRANSITION(deposite_balls);
                                } else {
                                        FSM_TRANSITION(next_carousel_position);
                                }
                        } else if (get_ball_attemps++ < 5) {
-                               FSM_TIMER(500);
+                               FSM_TIMER(1500);
                        } else {
-                               FSM_TRANSITION(go_to_container);
+                               printf("go_to_container\n");
+//                             FSM_TRANSITION(go_to_container);
                        }
                        break;
                case EV_BALL_INSIDE:
                        if (ball_inside)
                                break;
 
-                       printf("tady1\n");
                        ball_inside = 1;
                        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);
                        break;
                default: break;
@@ -420,7 +423,8 @@ FSM_STATE(next_carousel_position)
                        } else if (carousel_attempts < 3) {
                                FSM_TIMER(1000);
                        } else {
-                               FSM_TRANSITION(go_to_container);
+                               printf("go_to_container\n");
+//                             FSM_TRANSITION(go_to_container);
                        }
                        break;
                default: break;
@@ -481,10 +485,10 @@ FSM_STATE(go_to_container)
 FSM_STATE(deposite_balls)
 {
        int carousel_pos = 0;
+       int wait_for_deposition = 0;
 
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       open_back_door();
                        set_carousel(carousel_pos++);
 //                     robot.carousel_cnt--;
 //                     if (robot.carousel_cnt <= 0) {
@@ -497,11 +501,18 @@ FSM_STATE(deposite_balls)
                        FSM_TIMER(500);
                        break;
                case EV_TIMER:
+                       if (!wait_for_deposition) {
+                               open_back_door();
+                               wait_for_deposition = 1;
+                       }
+
+                       FSM_TIMER(500);
                        if (carousel_pos > 4)
-                               FSM_TRANSITION(go_to_our_white_dispenser);
+                               printf("ok\n");
+//                             FSM_TRANSITION(go_to_our_white_dispenser);
 //                     FSM_TRANSITION(next_carousel_position2);
+                       close_back_door();
                        set_carousel(carousel_pos++);
-                       FSM_TIMER(500);
                        break;
                default: break;
        }