robofsm: Strategy

New strategy for odometry autocalibration. Robot goes back and calibrate its odometry.

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 8cc25dfb550acb8af0a936c377624dfada0f7162..7d33d226542cbd27cfcd74ed71b2daf46ddd4378 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -9,7 +9,8 @@ robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
 
 bin_PROGRAMS += competition
 competition_SOURCES = competition2012.cc       \
-                     common-states.cc strategy_get_central_buillon.cc
+                     common-states.cc strategy_get_central_buillon.cc \
+                     strategy_odo_calibration.cc
 
 bin_PROGRAMS += homologation
 homologation_SOURCES = homologation2012.cc

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index ad31ad135076be0b7cb26af34a7cd8abc49717cd..7fd646d5c7b510dcebc6952ae8a11f4144487e96 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -421,4 +421,53 @@ FSM_STATE(leave_totem_up)
                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.5, NO_TURN(), &tcSlow);
+                       FSM_TIMER(500);
+                       break;
+               case EV_MOTION_DONE:
+                       ROBOT_LOCK(est_pos_odo);
+                       robot.odo_cal_a = 1/robot.odo_distance_a;
+                       robot.odo_cal_b = 1/robot.odo_distance_b;
+                       x1 = robot.odo_distance_a;
+                       y1 = robot.odo_distance_b;
+                       ROBOT_UNLOCK(est_pos_odo);
+                       printf("EROOR%f", x1);
+                       FILE * file;
+                       file = fopen ("odometry_cal_data","a+");
+                       sprintf(buffer, "%4.4f", 1/x1);
+                       fputs (buffer,file);
+                       fputs ("\n", file);
+                       sprintf(buffer, "%4.4f", 1/y1);
+                       fputs (buffer,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(500);
+                       }
+                       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 e17520f0e0c75dfa201fed612f51fb8bde35ee97..7dc1c4d068a099868e7fffa042761e7f7cb54f25 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -12,7 +12,7 @@ extern bool up;
 /* strategy FSM */
 FSM_STATE_DECL(get_central_buillon_first);
 FSM_STATE_DECL(ignore_central_buillon);
-//FSM_STATE_DECL(calibrate);
+FSM_STATE_DECL(calibrate);
 
 /* Strategy catch buillon in center */
 FSM_STATE_DECL(approach_central_buillon);
@@ -25,6 +25,9 @@ FSM_STATE_DECL(return_home);
 //FSM_STATE_DECL(push_nearest_buillon);
 //FSM_STATE_DECL(push_bottle_fw);
 
+/* Autocalibration */
+FSM_STATE_DECL(go_back_for_cal);
+
 /* Common states */
 FSM_STATE_DECL(leave_home);
 FSM_STATE_DECL(leave_home_for_totem);

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index 9ed85bb45d5a5e753277f3fc940293d4cde7c510..64d1e2186eade497da8a95315b18daa1478fd710 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -1,7 +1,7 @@
  /*
  * competition.cc       12/02/28
  *
- * Robot's control program intended for homologation (approval phase) on Eurobot 2012.
+ * Robot's control program intended for competition on Eurobot 2012.
  *
  * Copyright: (c) 2012 CTU Flamingos
  *            CTU FEE - Department of Control Engineering
@@ -39,7 +39,7 @@ 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);
 
@@ -49,7 +49,7 @@ int main()
        robot.fsm.motion.debug_states = 1;
        //robot.fsm.act.debug_states = 1;
 
-       robot.fsm.main.state = &fsm_state_main_get_central_buillon_first;
+       robot.fsm.main.state = &fsm_state_main_calibrate;
        //robot.fsm.main.transition_callback = trans_callback;
        //robot.fsm.motion.transition_callback = move_trans_callback;
 
@@ -78,4 +78,4 @@ int main()
        robot_destroy();
 
        return 0;
-} 
\ No newline at end of file
+}
\ No newline at end of file

diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
new file mode 100644 (file)
index 0000000..40987cf
--- /dev/null
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -0,0 +1,28 @@
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+
+UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name of the file */
+
+
+FSM_STATE(calibrate)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       FSM_TIMER(2000);
+                       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:;
+       }
+}
+