competition.cc: collecting four pucks sequence; automaton works somehow

diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc
index 4aa2758bcec5adcc1ee278a491db85179de2a4e6..201534a1188c028903dd6f606dba8bd1ad32ca4a 100644 (file)
--- a/src/robofsm/competition.cc
+++ b/src/robofsm/competition.cc
@@ -58,7 +58,7 @@ typedef enum {
  * Trajectory constraints used, are initialized in the init state
  ************************************************************************/
 
-struct TrajectoryConstraints tcFast, tcSlow;
+struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
 /************************************************************************
  * Variables related to puck collecting
@@ -75,41 +75,41 @@ int pucks_at_once; // number of pucks to load at once (maximum number of pucks w
 const int free_puck_pick_up_sequence[][6][3] = {
 /* nx, ny, arrival angle */
        { /* lot 1 */
-               {0, 3, 135},            
+               {0, 3, 135},
                {1, 3, 180},
-               {0, 0, 90},
-               {1, 0, 180},            
-               {2, 3, 270},
-               {2, 0, 270},
+               {2, 3, 180},
+               {2, 0, 90},
+               {1, 0, 45},
+               {0, 0, 0},
        },
        {
                {0, 3, 135},
-               {2, 3, 180},
+               {2, 3, 90},
                {2, 2, 90},
                {2, 1, 90},
-               {2, 0, 90},
-               {0, 0, 10},
+               {2, 0, 90},
+               {0, 0, 10},
        },
        {
                {0, 3, 135},
-               {2, 3, 180},
-               {1, 2, 90},
+               {2, 3, 160},
+               {1, 2, 40},
                {1, 1, 90},
-               {2, 0, 90},
-               {0, 0, 10},
+               {2, 0, 135},
+               {0, 0, 0},
        },
        { /* lot 4 */
                {0, 3, 135},
-               {2, 3, 180},
                {0, 2, 90},
                {0, 1, 90},
                {0, 0, 90},
-               {2, 0, 180},
+               {2, 3, -90},
+               {2, 0, 90},
        },
        {
                {0, 3, 135},
                {1, 3, 180},
-               {2, 2, 90},
+               {2, 2, 135},
                {2, 1, 90},
                {1, 0, 0},
                {0, 0, 0},
@@ -117,57 +117,56 @@ const int free_puck_pick_up_sequence[][6][3] = {
        { /* lot 6 */
                {0, 3, 135},
                {1, 3, 180},
-               {1, 2, 90},
+               {1, 2, 135},
                {1, 1, 90},
-               {1, 0, 0},
+               {1, 0, 0},
                {0, 0, 0},
        },
        {
                {0, 3, 135},
-               {1, 3, 180},
                {0, 2, 90},
                {0, 1, 90},
-               {1, 0, 45},
-               {0, 0, 0},
+               {0, 0, 90},
+               {1, 3, -45},
+               {1, 0, 45},
        },
        { /* lot 8 */
                {0, 3, 135},
-               {0, 0, 90},
-               {1, 1, 270},
-               {1, 2, 270},
-               {2, 2, 90},
-               {2, 1, 90},
+               {1, 2, 135},
+               {1, 1, 90},
+               {0, 0, 45},
+               {2, 2, -90},
+               {2, 1, -90},
        },
        {
                {0, 3, 135},
-               {2, 2, 135},
-               {2, 1, 90},
-               {0, 0, 45},
-               {0, 1, 300},    
-               {0, 2, 270},
-
+               {0, 2, 90},
+               {0, 1, 90},
+               {0, 0, 90},
+               {2, 2, -90},
+               {2, 1, -90},
        },
        { /* lot 10 */
                {0, 3, 135},
-               {1, 2, 135},
-               {1, 1, 45},
-               {0, 0, 45},
-               {0, 1, 300},
-               {0, 2, 270},
+               {0, 2, 90},
+               {0, 1, 90},
+               {0, 0, 90},
+               {1, 1, -45},
+               {1, 2, -90},
        },
 };
 
 const int prefered_acropolis_approach_angles[][3] = {
-       {180, 240, 130},
-       {130, 170, 210},
-       {130, 170, 210},
-       {130, 170, 210}, // FIXME... fix these
-       {130, 170, 210}, // FIXME... fix these
-       {130, 170, 210}, // FIXME... fix these
-       {130, 170, 210}, // FIXME... fix these
-       {220, 130, 190}, // FIXME... fix these
-       {130, 220, 180}, // FIXME... fix these
-       {130, 220, 180}, // FIXME... fix these
+       {180, 180, 220},
+       {130, 160, 200},
+       {130, 160, 200},
+       {130, 160, 200}, // FIXME... fix these
+       {130, 160, 200}, // FIXME... fix these
+       {130, 160, 200}, // FIXME... fix these
+       {130, 160, 200}, // FIXME... fix these
+       {130, 160, 200}, // FIXME... fix these
+       {130, 160, 200}, // FIXME... fix these
+       {130, 160, 200}, // FIXME... fix these
 };
 
 /************************************************************************
@@ -310,11 +309,13 @@ FSM_STATE(init)
        case EV_ENTRY:
                act_camera_on();
                free_puck_to_try_to_get_next = 0; // next free puck number (index of the free_puck_pick_up_sequence array) to pick up
-               pucks_at_once = 2; // number of pucks to load at once (maximum number of pucks we want to have in robot)
+               pucks_at_once = 4; // number of pucks to load at once (maximum number of pucks we want to have in robot)
                tcFast = trajectoryConstraintsDefault;
                //tcFast.maxv = 1.5;
                tcSlow = trajectoryConstraintsDefault;
                tcSlow.maxv = 0.2;
+               tcVerySlow = trajectoryConstraintsDefault;
+               tcVerySlow.maxv =  0.1;
                FSM_TRANSITION(wait_for_start);
                break;
        default:
@@ -398,34 +399,43 @@ FSM_STATE(decide_what_next)
 
 FSM_STATE(deposit_at_acropolis)
 {
-       static int deposit_status = 0;
+       static int deposit_status;
        switch (FSM_EVENT) {
                case EV_ENTRY:
+                       deposit_status = 0;
                        SUBFSM_TRANSITION(approach_acropolis, NULL);
                        break;
-               case EV_RETURN:
+               case EV_RETURN: {
                        //if ((bool)FSM_EVENT_INT == true) {
-                               FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, (void*) 2);
+                               int floor = 2;
+                               FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, (void*)floor);
                        //}
+                       }
                        break;
                case EV_ACTION_DONE:
                        switch(deposit_status) {
                        case 0:
-                               robot_move_by(0.05, NO_TURN(), &tcSlow);
+                               robot_move_by(0.11, NO_TURN(), &tcSlow);
                                break;
                        case 1:
-                               robot_move_by(0.07, NO_TURN(), &tcFast);
+                               robot_move_by(-0.07, NO_TURN(), &tcSlow);
                                break;
                        }
                        deposit_status++;
                        break;
                case EV_MOTION_DONE:
                        switch(deposit_status) {
-                       case 0:
+                       case 1:
                                FSM_SIGNAL(ACT, EV_UNLOAD_PUCKS, NULL);
                                break;
-                       case 1:
-                               FSM_SIGNAL(ACT, EV_FREE_SPACE, NULL);
+                       case 2:
+                               //FSM_SIGNAL(ACT, EV_FREE_SPACE, NULL);
+                               robot_move_by(0.08, NO_TURN(), &tcSlow);
+                               deposit_status++;
+                               break;
+                       case 3: 
+                               robot_move_by(-0.1, NO_TURN(), &tcSlow);
+                               deposit_status++;
                                break;
                        }
                        break;
@@ -492,7 +502,7 @@ FSM_STATE(approach_acropolis)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       robot_goto_acropolis(prefered_acropolis_approach_angles[(int)robot.game_conf][(free_puck_to_try_to_get_next+1)/2]);
+                       robot_goto_acropolis(prefered_acropolis_approach_angles[(int)robot.game_conf][(free_puck_to_try_to_get_next-1)/2]);
                        break;
                case EV_MOTION_DONE:
                        SUBFSM_RET(NULL);