robofsm: Separation of homologation, calibration and competition states.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 7db13184fab6e0cf7bc067240d89f58df40d78f5..cd896d24ef986f1255acbb9799d4a1a92360bc1a 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -421,106 +421,4 @@ FSM_STATE(leave_totem_up)
                default:
                        break;
        }
-}
-
-/* Homologation states */
-FSM_STATE(homolog_approach_buillon)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
-                       robot_trajectory_add_point_trans(
-                               0.65,
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
-                       robot_trajectory_add_point_trans(
-                               0.65,
-                               1.3);
-                       robot_trajectory_add_final_point_trans(
-                               0.5,
-                               1.1,
-                               NO_TURN());
-                       break;
-               case EV_MOTION_DONE:
-               case EV_TIMER:
-                       FSM_TRANSITION(homolog_push_bottle);
-                       break;
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(homolog_push_bottle)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
-                       robot_trajectory_add_point_trans(
-                               0.64,
-                               0.7);
-                       robot_trajectory_add_final_point_trans(
-                               0.64 + 0.08,
-                               ROBOT_AXIS_TO_FRONT_M + 0.05,
-                               ARRIVE_FROM(DEG2RAD(270), 0.10));
-                       break;
-               case EV_MOTION_DONE:
-                       SUBFSM_RET(NULL);
-                       break;
-               default:
-                       break;
-       }
-}
-
-
-/* State for odometry calibration */
-
-FSM_STATE(go_back_for_cal)
-{
-       double x1 = 0, y1 = 0;
-       char buffer [20];
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
-                                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
-                                               0);
-                       robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
-                       FSM_TIMER(200);
-                       break;
-               case EV_MOTION_DONE:
-                       ROBOT_LOCK(est_pos_odo);
-                       robot.odo_cal_a = -1.0/robot.odo_distance_a;
-                       robot.odo_cal_b = -1.0/robot.odo_distance_b;
-                       x1 = robot.odo_distance_a;
-                       y1 = robot.odo_distance_b;
-                       ROBOT_UNLOCK(est_pos_odo);
-                       if(x1 != 0 || y1 != 0) {
-                               printf("Distance for calibration: \n");
-                               printf("Left%f\n", x1);
-                               printf("Right%f\n", y1);
-                               FILE * file;
-                               file = fopen ("odometry_cal_data","a+");
-                               sprintf(buffer, "%4.4f", -1/x1);
-                               fputs (buffer,file);
-                               fputs (" ", file);
-                               sprintf(buffer, "%4.4f", -1/y1);
-                               fputs (buffer,file);
-                               fputs ("\n", file);
-                               fclose(file);
-                       } 
-                       SUBFSM_RET(NULL);
-                       break;
-               case EV_TIMER:
-                       ROBOT_LOCK(est_pos_odo);
-                       if(x1 == robot.odo_distance_a){
-                               x1 = robot.odo_distance_a;
-                               y1 = robot.odo_distance_b;
-                               FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
-                               FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
-                       } else {
-                               FSM_TIMER(200);
-                       }
-                       ROBOT_UNLOCK(est_pos_odo);
-                       break;
-               default:
-                       break;
-       }
 }
\ No newline at end of file

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 029684e818605f6029f3f21fd9c081f5f9e7eec2..2011e6c42fee9d97b801e26147cb535a7a9baeb9 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -11,7 +11,7 @@ extern double totem_x, totem_y;
 extern bool up;
 /* strategy FSM */
 FSM_STATE_DECL(get_central_buillon_first);
-FSM_STATE_DECL(homolog_wait_for_start);
+FSM_STATE_DECL(homologation_wait_for_start);
 FSM_STATE_DECL(calibrate);
 
 /* Strategy catch buillon in center */
@@ -49,9 +49,9 @@ FSM_STATE_DECL(push_second_bottle);
 */
 /* HOMOLOGATION states */
 /* movement states - buillon */
-FSM_STATE_DECL(homolog_approach_buillon);
+FSM_STATE_DECL(homologation_approach_buillon);
 /* Pushing the bottle */
-FSM_STATE_DECL(homolog_push_bottle);
+FSM_STATE_DECL(homologation_push_bottle);
 void start_entry();
 void start_timer();
 void start_go();

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index f9bb0a86a6537de1fd31d894717918b1be3d5a10..de7c2f4759ebc6ab25fa27fa01798db17742314c 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -39,7 +39,6 @@ UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file *
 int main()
 {
        int rv;
-       //robot_calibrate_odometry(); // calibrate odometry
        rv = robot_init();
        if (rv) error(1, errno, "robot_init() returned %d\n", rv);
 

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 724a9d5a43a5660fb17ed5e74e6268c69545c14e..029dc9d2e1f92b7d1446e15f64aaf397723e9171 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -297,7 +297,7 @@ void robot_calibrate_odometry()
        { 
                robot.odo_cal_a = 1;
                robot.odo_cal_b = 1;
-               fprintf(stderr, "File not found. \n\n");
+               fprintf(stderr, "ODO calibration file not found! \n\n");
                return;
        }
        char line [15];
@@ -318,6 +318,6 @@ void robot_calibrate_odometry()
        }
        robot.odo_cal_a = a / num;
        robot.odo_cal_b = b / num;
-       printf("calibrated value left: %f\n",robot.odo_cal_a);
-       printf("calibrated value right: %f\n",robot.odo_cal_b);
+       printf("ODO calibrated value A: %f\n",robot.odo_cal_a);
+       printf("ODO calibrated value B: %f\n",robot.odo_cal_b);
 }

diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc
index d82a961a8c82e95722ba56948bcd32bbae20c613..ddf9315b0b65b62bbd7eedf45d5f7051bc4f78b1 100644 (file)
--- a/src/robofsm/strategy_get_central_buillon.cc
+++ b/src/robofsm/strategy_get_central_buillon.cc
@@ -19,6 +19,7 @@ FSM_STATE(get_central_buillon_first)
 //#endif
                case EV_START:
                        start_go();
+                        robot_calibrate_odometry();
                        FSM_TRANSITION(pick_central_buillon);
                        break;
                case EV_TIMER:
@@ -29,7 +30,7 @@ FSM_STATE(get_central_buillon_first)
                        start_exit();
                        break;
                case EV_SWITCH_STRATEGY:
-                       FSM_TRANSITION(homolog_wait_for_start);
+                       FSM_TRANSITION(homologation_wait_for_start);
                        break;
                default:;
        }

diff --git a/src/robofsm/strategy_homologation.cc b/src/robofsm/strategy_homologation.cc
index bea54155748a98440d4d236eed00d831ef74ba4a..7d2a9ade0994a4cd996f84d94ebe0a6aa6bbcb7c 100644 (file)
--- a/src/robofsm/strategy_homologation.cc
+++ b/src/robofsm/strategy_homologation.cc
@@ -1,12 +1,28 @@
-#include "common-states.h"
-#include "robot.h"
+#define FSM_MAIN
+#include "robodata.h"
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <sharp.h>
+#include <trgen.h>
+#include "match-timing.h"
+#include <stdbool.h>
 #include <ul_log.h>
-
+#include "common-states.h"
 
 UL_LOG_CUST(ulogd_strategy_homologation); /* Log domain name = ulogd + name of the file */
 
 
-FSM_STATE(homolog_wait_for_start)
+FSM_STATE(homologation_wait_for_start)
 {
        switch (FSM_EVENT) {
                case EV_ENTRY:
@@ -18,7 +34,8 @@ FSM_STATE(homolog_wait_for_start)
 //#endif
                case EV_START:
                        start_go();
-                       SUBFSM_TRANSITION(homolog_approach_buillon, NULL);
+                        robot_calibrate_odometry();
+                       FSM_TRANSITION(homologation_approach_buillon);
                        break;
                case EV_TIMER:
                        FSM_TIMER(1000);
@@ -34,4 +51,49 @@ FSM_STATE(homolog_wait_for_start)
                        break;
                default:;
        }
+}
+
+FSM_STATE(homologation_approach_buillon)
+{
+        switch(FSM_EVENT) {
+                case EV_ENTRY:
+                        robot_trajectory_new(&tcSlow); // new trajectory
+                        robot_trajectory_add_point_trans(
+                                0.65,
+                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+                        robot_trajectory_add_point_trans(
+                                0.65,
+                                1.3);
+                        robot_trajectory_add_final_point_trans(
+                                0.5,
+                                1.1,
+                                NO_TURN());
+                        break;
+                case EV_MOTION_DONE:
+                case EV_TIMER:
+                        FSM_TRANSITION(homologation_push_bottle);
+                        break;
+                default:
+                        break;
+        }
+}
+
+FSM_STATE(homologation_push_bottle)
+{
+        switch(FSM_EVENT) {
+                case EV_ENTRY:
+                        robot_trajectory_new(&tcSlow); // new trajectory
+                        robot_trajectory_add_point_trans(
+                                0.64,
+                                0.7);
+                        robot_trajectory_add_final_point_trans(
+                                0.64 + 0.08,
+                                ROBOT_AXIS_TO_FRONT_M + 0.05,
+                                ARRIVE_FROM(DEG2RAD(270), 0.10));
+                        break;
+                case EV_MOTION_DONE:
+                        break;
+                default:
+                        break;
+        }
 }
\ No newline at end of file

diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
index 0ce52f657398a8eac93221a7dc6d457708ee477a..877afae86796cee3ca3e5bdf9296d02742df1116 100644 (file)
--- a/src/robofsm/strategy_odo_calibration.cc
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -1,6 +1,24 @@
-#include "common-states.h"
-#include "robot.h"
+#define FSM_MAIN
+#include "robodata.h"
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <sharp.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include "actuators.h"
+#include <sharp.h>
+#include <trgen.h>
+#include "match-timing.h"
+#include <stdbool.h>
 #include <ul_log.h>
+#include "common-states.h"
+
 
 
 UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name of the file */
@@ -8,23 +26,87 @@ UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name o
 
 FSM_STATE(calibrate)
 {
-       switch (FSM_EVENT) {
-               case EV_ENTRY:
-                       FSM_TIMER(2000);
-                       robot.odo_cal_a = 1;
-                       robot.odo_cal_b = 1;
-                       break;
-               case EV_TIMER:
-                       SUBFSM_TRANSITION(go_back_for_cal,NULL);
-                       break;
-               case EV_RETURN:
-                       robot_calibrate_odometry();
-                       //FSM_TRANSITION(get_central_buillon_first);
-                       break;
-               case EV_SWITCH_STRATEGY:
-                       FSM_TRANSITION(get_central_buillon_first);
-                       break;
-               default:;
-       }
+        switch (FSM_EVENT) {
+                case EV_ENTRY:
+                        start_entry();
+//#ifdef COMPETITION
+                        ul_logmsg("waiting for start\n");
+                        FSM_TIMER(1000);
+                        break;
+//#endif
+                case EV_START:
+                        start_go();
+                        robot.obstacle_avoidance_enabled = false;
+                        robot.use_back_bumpers = false;
+                        robot.odo_cal_a = 1;
+                        robot.odo_cal_b = 1;
+                        FSM_TRANSITION(go_back_for_cal);
+                        break;
+                case EV_TIMER:
+                        FSM_TIMER(1000);
+                        start_timer();
+                        break;
+                case EV_EXIT:
+                        start_exit();
+                        break;
+                case EV_SWITCH_STRATEGY:
+                        FSM_TRANSITION(get_central_buillon_first);
+                        break;
+                default:;
+        }
+}
+
+/* State for odometry calibration */
+
+FSM_STATE(go_back_for_cal)
+{
+        double x1 = 0, y1 = 0;
+        char buffer [20];
+        switch (FSM_EVENT) {
+                case EV_ENTRY:
+                        robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
+                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
+                                                0);
+                        robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
+                        FSM_TIMER(50);
+                        break;
+                case EV_MOTION_DONE:
+                        ROBOT_LOCK(est_pos_odo);
+                        robot.odo_cal_a = -1.0/robot.odo_distance_a;
+                        robot.odo_cal_b = -1.0/robot.odo_distance_b;
+                        x1 = robot.odo_distance_a;
+                        y1 = robot.odo_distance_b;
+                        ROBOT_UNLOCK(est_pos_odo);
+                        if(x1 != 0 || y1 != 0) {
+                                printf("Distance for calibration: \n");
+                                printf("Left%f\n", x1);
+                                printf("Right%f\n", y1);
+                                FILE * file;
+                                file = fopen ("odometry_cal_data","a+");
+                                sprintf(buffer, "%4.4f", -1/x1);
+                                fputs (buffer,file);
+                                fputs (" ", file);
+                                sprintf(buffer, "%4.4f", -1/y1);
+                                fputs (buffer,file);
+                                fputs ("\n", file);
+                                fclose(file);
+                        } 
+                        FSM_TRANSITION(get_central_buillon_first);
+                        break;
+                case EV_TIMER:
+                        ROBOT_LOCK(est_pos_odo);
+                        if(x1 == robot.odo_distance_a){
+                                x1 = robot.odo_distance_a;
+                                y1 = robot.odo_distance_b;
+                                FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
+                                FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
+                        } else {
+                                FSM_TIMER(10);
+                        }
+                        ROBOT_UNLOCK(est_pos_odo);
+                        break;
+                default:
+                        break;
+        }
 }