robofsm: Move odometry calibration function to special file.

[eurobot/public.git] / src / robofsm / strategy_odo_calibration.cc

diff --git a/src/robofsm/strategy_odo_calibration.cc b/src/robofsm/strategy_odo_calibration.cc
index 0ce52f657398a8eac93221a7dc6d457708ee477a..b78750016ab99ae56e4a9f092a36516defaf8a58 100644 (file)
--- a/src/robofsm/strategy_odo_calibration.cc
+++ b/src/robofsm/strategy_odo_calibration.cc
@@ -1,30 +1,146 @@
-#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"
 
+void robot_calibrate_odometry()
+{
+        robot.odo_distance_a = 0;
+        robot.odo_distance_b = 0;
+        FILE *file;
+        file = fopen ("odometry_cal_data", "r");
+        if (file == NULL)
+        {
+                robot.odo_cal_a = 1;
+                robot.odo_cal_b = 1;
+                fprintf(stderr, "ODO calibration file not found! \n\n");
+                return;
+        }
+        char line [15];
+        float a = 0;
+        float b = 0;
+        int num = 0;
+        while (fgets (line, 15 , file)) {
+                a += atof(strtok(line," "));
+                fgets (line, 15 , file);
+                b += atof(strtok(NULL," "));
+                num ++;
+                printf("a: %i, b: %i, num: %i\n", a, b, num);
+        }
+        fclose (file);
+        if(a == 0 || b == 0) {
+                robot.odo_cal_a = 1;
+                robot.odo_cal_b = 1;
+                return;
+        }
+        robot.odo_cal_a = a / num;
+        robot.odo_cal_b = b / num;
+        printf("ODO calibrated value A: %f\n",robot.odo_cal_a);
+        printf("ODO calibrated value B: %f\n",robot.odo_cal_b);
+}
 
 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);
-                       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;
+        }
 }