Odometry autocalibration

[eurobot/public.git] / src / robofsm / common-states.cc

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index ad31ad135076be0b7cb26af34a7cd8abc49717cd..f0b910d8dbb21057d8ffbc74a84792e9034fa2d3 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -421,4 +421,55 @@ 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.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);
+                       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 ("\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(200);
+                       }
+                       ROBOT_UNLOCK(est_pos_odo);
+                       break;
+               default:
+                       break;
+       }
 }
\ No newline at end of file