src/robofsm/strategy_odo_calibration.cc

   1 #define FSM_MAIN
   2 #include "robodata.h"
   3 #include <robot.h>
   4 #include <fsm.h>
   5 #include <unistd.h>
   6 #include <math.h>
   7 #include <movehelper.h>
   8 #include <map.h>
   9 #include <sharp.h>
  10 #include <robomath.h>
  11 #include <string.h>
  12 #include <robodim.h>
  13 #include <error.h>
  14 #include "actuators.h"
  15 #include <sharp.h>
  16 #include <trgen.h>
  17 #include "match-timing.h"
  18 #include <stdbool.h>
  19 #include <ul_log.h>
  20 #include "common-states.h"
  21
  22
  23
  24 UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name of the file */
  25
  26
  27 FSM_STATE(calibrate)
  28 {
  29         switch (FSM_EVENT) {
  30                 case EV_ENTRY:
  31                         start_entry();
  32 //#ifdef COMPETITION
  33                         ul_logmsg("waiting for start\n");
  34                         FSM_TIMER(1000);
  35                         break;
  36 //#endif
  37                 case EV_START:
  38                         start_go();
  39                         robot.obstacle_avoidance_enabled = false;
  40                         robot.use_back_bumpers = false;
  41                         robot.odo_cal_a = 1;
  42                         robot.odo_cal_b = 1;
  43                         FSM_TRANSITION(go_back_for_cal);
  44                         break;
  45                 case EV_TIMER:
  46                         FSM_TIMER(1000);
  47                         start_timer();
  48                         break;
  49                 case EV_EXIT:
  50                         start_exit();
  51                         break;
  52                 case EV_SWITCH_STRATEGY:
  53                         FSM_TRANSITION(get_central_buillon_first);
  54                         break;
  55                 default:;
  56         }
  57 }
  58
  59 /* State for odometry calibration */
  60
  61 FSM_STATE(go_back_for_cal)
  62 {
  63         double x1 = 0, y1 = 0;
  64         char buffer [20];
  65         switch (FSM_EVENT) {
  66                 case EV_ENTRY:
  67                         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
  68                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
  69                                                 0);
  70                         robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
  71                         FSM_TIMER(50);
  72                         break;
  73                 case EV_MOTION_DONE:
  74                         ROBOT_LOCK(est_pos_odo);
  75                         robot.odo_cal_a = -1.0/robot.odo_distance_a;
  76                         robot.odo_cal_b = -1.0/robot.odo_distance_b;
  77                         x1 = robot.odo_distance_a;
  78                         y1 = robot.odo_distance_b;
  79                         ROBOT_UNLOCK(est_pos_odo);
  80                         if(x1 != 0 || y1 != 0) {
  81                                 printf("Distance for calibration: \n");
  82                                 printf("Left%f\n", x1);
  83                                 printf("Right%f\n", y1);
  84                                 FILE * file;
  85                                 file = fopen ("odometry_cal_data","a+");
  86                                 sprintf(buffer, "%4.4f", -1/x1);
  87                                 fputs (buffer,file);
  88                                 fputs (" ", file);
  89                                 sprintf(buffer, "%4.4f", -1/y1);
  90                                 fputs (buffer,file);
  91                                 fputs ("\n", file);
  92                                 fclose(file);
  93                         }
  94                         FSM_TRANSITION(get_central_buillon_first);
  95                         break;
  96                 case EV_TIMER:
  97                         ROBOT_LOCK(est_pos_odo);
  98                         if(x1 == robot.odo_distance_a){
  99                                 x1 = robot.odo_distance_a;
 100                                 y1 = robot.odo_distance_b;
 101                                 FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
 102                                 FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 103                         } else {
 104                                 FSM_TIMER(10);
 105                         }
 106                         ROBOT_UNLOCK(est_pos_odo);
 107                         break;
 108                 default:
 109                         break;
 110         }
 111 }
 112