dev commit.

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index 808c38750d6b9c4a88bd5a219b91d2abd3242e7d..33ea1e228389afba234339fe662e1a2f198d074b 100644 (file)
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -131,27 +131,6 @@ void get_back_sharp_m(double *sharp)
        ROBOT_UNLOCK(sharps);
 }
 
-/**
- * Detection of the enemy ahead.
- *
- * @return     enemy may be ahead us.
- */
-int enemy_ahead()
-{
-       double sharp[3];
-       int cnt = 0, i;
-
-       get_front_sharp_m(sharp);
-       printf("front sharp: %f %f\n", sharp[LEFT], sharp[RIGHT]);
-       for (i=0; i<2; i++)
-               if (sharp[i] > 0 && sharp[i] < 0.30)
-                       cnt++;
-
-       /* FIXME: dont use enemy avoidance now */
-//     return ((cnt >= 1) ? 1 : 0);
-       return (0);
-}
-
 /**
  * Use bumpers check if we are closed to the dispenser
  */
@@ -270,13 +249,13 @@ FSM_STATE(init)
                        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);
                        /* FIXME: delete after the test */
-                       robot.carousel_cnt = 5;
-                       robot.carousel_pos = 0;
-                       FSM_TRANSITION(deposite_balls);
+//                     robot.carousel_cnt = 5;
+//                     robot.carousel_pos = 0;
+//                     FSM_TRANSITION(deposite_balls);
                        break;
                default: break;
        }
@@ -324,7 +303,43 @@ FSM_STATE(go_to_our_white_dispenser)
 
 FSM_STATE(go_to_our_white_dispenser2)
 {
-       struct ref_pos_type des_pos = { 0.20, PLAYGROUND_HEIGHT_M - 0.65, 0};
+       struct ref_pos_type des_pos = {0.20, PLAYGROUND_HEIGHT_M - 0.65, 0};
+       static int approaching_attempts = 0;
+
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_goto_point(des_pos);
+                       break;
+               case EV_MOTION_DONE:
+                       if (closed_to_dispenser() || approaching_attempts++ < 2) {
+                               FSM_TRANSITION(go_from_our_dispenser);
+                       } else {
+                               FSM_TRANSITION(get_balls);
+                               approaching_attempts = 0;
+                       }
+                       break;
+               default: break;
+       }
+}
+
+FSM_STATE(go_to_our_red_dispenser)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(NULL);
+                       robot_trajectory_add_final_point_trans(0.7, 
+                                       PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(go_to_our_white_dispenser2);
+                       break;
+               default: break;
+       }
+}
+
+FSM_STATE(go_to_our_red_dispenser2)
+{
+       struct ref_pos_type des_pos = {0.65, PLAYGROUND_HEIGHT_M - 0.20, 0};
        static int approaching_attempts = 0;
 
        switch (FSM_EVENT) {
@@ -362,9 +377,7 @@ FSM_STATE(go_from_our_dispenser)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new_backward(NULL);
-                       robot_trajectory_add_final_point_trans(0.2, 
-                                       PLAYGROUND_HEIGHT_M - 0.7, NO_TURN());
+                       /* FIXME: go backward by */
                        break;
                case EV_MOTION_DONE:
                        FSM_TRANSITION(go_to_our_white_dispenser2);
@@ -508,8 +521,6 @@ FSM_STATE(deposite_balls)
                        if (robot.carousel_pos < CAROUSEL_SIZE) {
                                close_back_door();
                                SUBFSM_TRANSITION(next_carousel_position, NULL);
-                               robot.carousel_cnt--;
-                               robot.carousel_pos++;
                        } else {
                                robot.carousel_cnt = 0;
                                /* FIXME: */
@@ -523,6 +534,8 @@ FSM_STATE(deposite_balls)
                        FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
                        break;
                case EV_TIMER:
+                       robot.carousel_cnt--;
+                       robot.carousel_pos++;
                        close_back_door();
                        DBG("carousel_cnt = %d\n", robot.carousel_cnt);
                        FSM_TRANSITION(deposite_balls);

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index dde70c319fc32d850c5dccdda62cee7c06361e12..ef06bf2f385be8c25868101858e479737ca8c73f 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -138,7 +138,7 @@ struct robot_eb2008 {
        unsigned char beacon_color;
 
        #define CAROUSEL_SIZE 5
-       #define BALL_TO_COLLECT 5
+       #define BALL_TO_COLLECT 1
        enum ball_color carousel[CAROUSEL_SIZE];
        unsigned char carousel_cnt; /* The number of balls in carousel */
        unsigned char carousel_pos; /* required position of the carousel */

diff --git a/src/robofsm/eb2008/test/Makefile.omk b/src/robofsm/eb2008/test/Makefile.omk
index 283c37c1330e3080fec59a093fc660207d9bfce1..0a6573ecc1a5e2d3c17299a050e6b1f0486b4d9a 100644 (file)
--- a/src/robofsm/eb2008/test/Makefile.omk
+++ b/src/robofsm/eb2008/test/Makefile.omk
@@ -18,6 +18,9 @@ display_SOURCES = display.cc
 test_PROGRAMS += mcl-laser
 mcl-laser_SOURCES = mcl-laser.cc
 
+test_PROGRAMS += homologation
+homologation_SOURCES = homologation.cc
+
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot_eb2008 mcl robodim_eb2008 laser-nav robomath uoled oledlib sercom roboorte_generic roboorte_eb2008 \
                robottype robottype_eb2008 pthread \