Homologation - approved version.

robofsm: Jaw actuator positions tuning. Disabled shapedet function.
pathplaner: Remove safety margin around obstacle.

We moved hokuyo sensor upward so it does not scan pawns. There was absolutely no free space between figures.
We were trying to test and use shapedet functionality, but it was not as good as we expected.
The biggest problem is that we did not read a Eurobot rules properly - there is that the testing (fake) opponent robot is 200mm in diameter.
This means that the shapedet library is tunned to detect the opponent robot as good as playing elements - LOL.

diff --git a/src/pathplan/path_planner.c b/src/pathplan/path_planner.c
index 56f37e95049092c1fab5d7af49cfc6fe873fdff7..dc533cd35cd89caea2eb4ed25b6450cf1975a13e 100644 (file)
--- a/src/pathplan/path_planner.c
+++ b/src/pathplan/path_planner.c
@@ -84,7 +84,7 @@ int path_planner(double xstart_real,double ystart_real, double xgoal_real, doubl
                return PP_ERROR_MAP_NOT_INIT;
        }
 
-       add_safety_margin();
+       //add_safety_margin();
 
        // If the goal map is not free, abort.
        if (!ShmapIsFreePoint(xgoal_real, ygoal_real)) {

diff --git a/src/robofsm/actuators.h b/src/robofsm/actuators.h
index 4997c676012faeb51b717abc8f5e7b22dd8ef91e..a2e25590d72838690d5461d333ce0e42a6ac0fdd 100644 (file)
--- a/src/robofsm/actuators.h
+++ b/src/robofsm/actuators.h
@@ -39,14 +39,14 @@ typedef enum {
   CATCH
 } jaws_cmds;
 
-#define JAW_LEFT_OPEN  0x1C3
-#define JAW_RIGHT_OPEN 0x1B3
+#define JAW_LEFT_OPEN  0x1E0
+#define JAW_RIGHT_OPEN 0x1EB
 
-#define JAW_LEFT_CLOSE 0x31F
-#define JAW_RIGHT_CLOSE        0x37F
+#define JAW_LEFT_CLOSE 0x2EE
+#define JAW_RIGHT_CLOSE        0x364
 
-#define JAW_LEFT_CATCH 0x25F
-#define JAW_RIGHT_CATCH 0x27F
+#define JAW_LEFT_CATCH 0x231
+#define JAW_RIGHT_CATCH        0x25F
 
 #ifdef __cplusplus
 extern "C" {

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index a63dc3c1b73770c629b55c9dd04ade5f7b697e80..96fe60b5c15cc972e0ef9635e5943ece386ec2f7 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -49,7 +49,8 @@ FSM_STATE_DECL(wait_for_start);
 FSM_STATE_DECL(aproach_first_fugure);
 FSM_STATE_DECL(load_first_figure);
 FSM_STATE_DECL(to_red_square);
-FSM_STATE_DECL(to_container_ortho);
+FSM_STATE_DECL(go_home);
+FSM_STATE_DECL(go_out);
 // FSM_STATE_DECL(experiment_decider);
 // FSM_STATE_DECL(approach_next_corn);
 // FSM_STATE_DECL(rush_the_corn);
@@ -64,17 +65,10 @@ FSM_STATE(init)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
-                       tcFast = trajectoryConstraintsDefault;
-                       tcFast.maxv = 0.8;
-                       tcFast.maxacc = 1;
-                       tcFast.maxomega = 2;
                        tcSlow = trajectoryConstraintsDefault;
-                       tcSlow.maxv = 0.5;
-                       tcSlow.maxacc = 0.5;
-                       tcVerySlow = trajectoryConstraintsDefault;
-                       tcVerySlow.maxv = 0.05;
-                       tcVerySlow.maxacc = 0.05;
-
+                       tcSlow.maxv = 0.3;
+                       tcSlow.maxacc = 0.3;
+                       tcSlow.maxomega = 1;
                        FSM_TRANSITION(wait_for_start);
                        break;
                default:
@@ -195,6 +189,7 @@ FSM_STATE(aproach_first_fugure)
                        robot.use_back_bumpers = false;
 
                        robot_trajectory_new(&tcSlow);
+                       //robot_move_by(0.5, NO_TURN(), &tcSlow);
                        robot_trajectory_add_point_trans(
                                0.4+0.05+0.25,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05);
@@ -204,7 +199,7 @@ FSM_STATE(aproach_first_fugure)
                                TURN_CCW(0));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(3000);
+                       FSM_TIMER(2000);
                        act_jaws(OPEN);
                        //FSM_TRANSITION(load_first_figure);
                        break;
@@ -238,6 +233,7 @@ FSM_STATE(load_first_figure)
                case EV_START:
                case EV_TIMER:
                        FSM_TRANSITION(to_red_square);
+                       break;
                case EV_RETURN:
                //case EV_VIDLE_DONE:
                case EV_MOTION_ERROR:
@@ -256,6 +252,9 @@ FSM_STATE(to_red_square)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
+                       robot.use_left_bumper = true;
+                       robot.use_right_bumper = true;
+                       robot.use_back_bumpers = true;
                        robot_trajectory_new(&tcSlow);
                        // face the rim with front of the robot
                        //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
@@ -264,12 +263,12 @@ FSM_STATE(to_red_square)
                        robot_trajectory_add_final_point_trans(0.4+0.05+0.2 + ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05, 0.35+0.35+0.17, NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(5000);
+                       FSM_TIMER(2000);
                        act_jaws(OPEN);
                        break;
                case EV_TIMER:
                        //act_jaws(OPEN);
-                       //FSM_TRANSITION(wait_for_start);
+                       FSM_TRANSITION(go_out);
                        break;
                case EV_START:
                case EV_RETURN:
@@ -291,33 +290,49 @@ FSM_STATE(to_red_square)
 //
 // static struct corn *corn_to_get;
 //
-// FSM_STATE(experiment_decider)
-// {
-//
-//     switch(FSM_EVENT) {
-//             case EV_ENTRY:
-//                     if (where_to_go == CORN) {
-//                             FSM_TRANSITION(approach_next_corn);
-//                     } else if (where_to_go == CONTAINER) {
-//                             FSM_TRANSITION(rush_the_corn);
-//                     } else if (where_to_go == TURN_AROUND) {
-//                             FSM_TRANSITION(turn_around);
-//                     } else /* NO_MORE_CORN */ {
-//                     }
-//                     break;
-//             case EV_START:
-//             case EV_TIMER:
-//             case EV_RETURN:
-//             case EV_VIDLE_DONE:
-//             case EV_MOTION_DONE:
-//             case EV_MOTION_ERROR:
-//             case EV_SWITCH_STRATEGY:
-//                     DBG_PRINT_EVENT("unhandled event");
-//             case EV_EXIT:
-//                     break;
-//     }
-// }
+FSM_STATE(go_out)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_move_by(0.2, NO_TURN(), &tcSlow);
+                       // face the rim with front of the robot
+                       break;
+               case EV_START:
+               case EV_TIMER:
+                       FSM_TRANSITION(go_home);
+                       break;
+               case EV_RETURN:
+               case EV_MOTION_DONE:
+                       FSM_TIMER(2000);
+                       act_jaws(CLOSE);
+                       break;
+               case EV_MOTION_ERROR:
+               case EV_SWITCH_STRATEGY:
+                       DBG_PRINT_EVENT("unhandled event");
+               case EV_EXIT:
+                       break;
+       }
+}
 
+
+FSM_STATE(go_home)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow);
+                       robot_trajectory_add_final_point_trans(0.4, PLAYGROUND_HEIGHT_M - 0.2, ARRIVE_FROM(DEG2RAD(180), 0.20));
+                       break;
+               case EV_START:
+               case EV_TIMER:
+               case EV_RETURN:
+               case EV_MOTION_DONE:
+               case EV_MOTION_ERROR:
+               case EV_SWITCH_STRATEGY:
+                       DBG_PRINT_EVENT("unhandled event");
+               case EV_EXIT:
+                       break;
+       }
+}
 // static int cnt = 0;
 // FSM_STATE(approach_next_corn)
 // {

diff --git a/src/robofsm/map_handling.cc b/src/robofsm/map_handling.cc
index 2ad47318a59abc892bfb93950825492889304935..6f328d5977dc030f46022b7b0abb4a6493cdad53 100644 (file)
--- a/src/robofsm/map_handling.cc
+++ b/src/robofsm/map_handling.cc
@@ -223,37 +223,37 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
 
        double distance;
 
-       if (arcs.size() > 0) {
-               for (i = 0; i < arcs.size(); i++) {
-                       x = arcs[i].center.x / 1000;
-                       y = arcs[i].center.y / 1000;
-
-                       tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
-                       tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
-
-                       center_arcs.push_back(tmpPoint);
-               }
-
-               for (i = 0; i < center_arcs.size(); i++) {
-                       if (robot.team_color) {
-                               for (unsigned int j = 0; j < blues.size(); j++) {
-                                       distance = point_distance(blues[j], center_arcs[i]);
-                                       if (distance < 0.05) {
-                                               figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
-                                               break;
-                                       }
-                               }
-                       } else {
-                               for (unsigned int j = 0; j < reds.size(); j++) {
-                                       distance = point_distance(reds[j], center_arcs[i]);
-                                       if (distance < 0.05) {
-                                               figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
-                                               break;
-                                       }
-                               }
-                       }
-               }
-       }
+//     if (arcs.size() > 0) {
+//             for (i = 0; i < arcs.size(); i++) {
+//                     x = arcs[i].center.x / 1000;
+//                     y = arcs[i].center.y / 1000;
+//
+//                     tmpPoint.x = e.x + x * cos(e.phi) - y * sin(e.phi);
+//                     tmpPoint.y = e.y + x * sin(e.phi) + y * cos(e.phi);
+//
+//                     center_arcs.push_back(tmpPoint);
+//             }
+//
+//             for (i = 0; i < center_arcs.size(); i++) {
+//                     if (robot.team_color) {
+//                             for (unsigned int j = 0; j < blues.size(); j++) {
+//                                     distance = point_distance(blues[j], center_arcs[i]);
+//                                     if (distance < 0.05) {
+//                                             figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
+//                                             break;
+//                                     }
+//                             }
+//                     } else {
+//                             for (unsigned int j = 0; j < reds.size(); j++) {
+//                                     distance = point_distance(reds[j], center_arcs[i]);
+//                                     if (distance < 0.05) {
+//                                             figure_detected_at(center_arcs[i].x, center_arcs[i].y, true);
+//                                             break;
+//                                     }
+//                             }
+//                     }
+//             }
+//     }
 
        bool obstacle = true;
 
@@ -262,7 +262,7 @@ void update_map_hokuyo(struct hokuyo_scan_type *s)
                if((beam.ang<(-HOKUYO_RANGE_ANGLE_LEFT/180.0*M_PI))||((beam.ang>(HOKUYO_RANGE_ANGLE_RIGHT/180.0*M_PI))))
                        continue;
 
-               if(data[i] > 19 && data[i] < 4000) {
+               if(data[i] > 19 && data[i] < 2000) {
                        obst_coord(&e, &beam, data[i]/1000.0, &x, &y);
 
                        tmpPoint.x = x;