sharpcalib: changed

diff --git a/src/robofsm/eb2008/fsmmove.cc b/src/robofsm/eb2008/fsmmove.cc
index f98a586ee90a2481d7774e0d560238ade21e7e03..e85462fb093d0a736db41282a40c08b7b3cc4d24 100644 (file)
--- a/src/robofsm/eb2008/fsmmove.cc
+++ b/src/robofsm/eb2008/fsmmove.cc
@@ -44,7 +44,7 @@ const struct balet_params k = {
 #define SIG_NEWDATA (SIGRTMIN+1)
 
 // Global varibles
-static pthread_t thr_trajctory_follower;
+static pthread_t thr_trajectory_follower;
 static struct timeval tv_start; /**< Absolute time, when trajectory started. */
 
 // Trajectory recalculation thread and synchoronization
@@ -165,9 +165,9 @@ static void do_control()
                // supposed that the estimated position is
                // equal to the reference position.
                robot_set_est_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
-               est_pos.x = robot.est_pos.x;
-               est_pos.y = robot.est_pos.y;
-               est_pos.phi = robot.est_pos.phi;
+               est_pos.x = ref_pos.x;
+               est_pos.y = ref_pos.y;
+               est_pos.phi = ref_pos.phi;
 #endif
 
 #ifdef NEVER //DEBUG
@@ -264,7 +264,7 @@ static void go(Trajectory *t)
 static void stop()
 {
        delete_actual_trajectory();
-       pthread_kill(thr_trajctory_follower, SIG_NEWDATA);
+       pthread_kill(thr_trajectory_follower, SIG_NEWDATA);
 }
 
 enum target_status {
@@ -432,7 +432,7 @@ FSM_STATE(init)
                        perror("move_init: pthread_attr_setschedparam(follower)");
                        exit(1);
                }
-               ret = pthread_create(&thr_trajctory_follower, &tattr, thread_trajectory_follower, NULL);
+               ret = pthread_create(&thr_trajectory_follower, &tattr, thread_trajectory_follower, NULL);
                if(ret) {
                        perror("move_init: pthread_create");
                        exit(1);

diff --git a/src/robofsm/eb2008/test/sharpcalib.cc b/src/robofsm/eb2008/test/sharpcalib.cc
index 7dca94b39bf76bef2a6a3404515fa32fedbb0d57..ea9dc1545b18c71b93a03442ce69004c095e3b90 100644 (file)
--- a/src/robofsm/eb2008/test/sharpcalib.cc
+++ b/src/robofsm/eb2008/test/sharpcalib.cc
@@ -5,21 +5,58 @@
 #include <trgen.h>
 #include <robomath.h>
 
+FSM_STATE_DECL(measure);
+FSM_STATE_DECL(move);
+
+
 FSM_STATE(init)
 {
-       static int cm = 100;
        switch (FSM_EVENT) {
                case EV_ENTRY:
+//                     brushes_drives_in();
+//                     on_bagr();
                        robot_set_est_pos_trans(1, 0.5, DEG2RAD(180));
                        FSM_TIMER(1000);
                        break;
                case EV_TIMER:
-               case EV_MOTION_DONE:
-                       printf("%3d cm: sharp left: %5.3f right: %5.3f\n", cm, robot.sharps.front_left, robot.sharps.front_right);
+                       FSM_TRANSITION(measure);
+                       break;
+               default:
+                       break;
+       }
+}
+
+int cm = 100;
+
+FSM_STATE(move)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       if (cm <= 10)
+                               robot_exit();
                        robot_move_by(0.01, NO_TURN(), NULL);
+                       break;
+               case EV_MOTION_DONE:
                        cm--;
-                       if (cm == 0)
-                               robot_exit();
+                       FSM_TRANSITION(measure);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(measure)
+{
+       static int count;
+       FSM_TIMER(50);
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       count = 0;
+               case EV_TIMER:
+                       printf("%3d %9.3f %9.3f\n", cm, robot.sharps.front_left, robot.sharps.front_right);
+                       count++;
+                       if (count >= 20)
+                               FSM_TRANSITION(move);
                        break;
                default:
                        break;