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 void robot_calibrate_odometry()
  23 {
  24         robot.odo_distance_a = 0;
  25         robot.odo_distance_b = 0;
  26         FILE *file;
  27         file = fopen ("odometry_cal_data", "r");
  28         if (file == NULL)
  29         {
  30                 robot.odo_cal_a = 1;
  31                 robot.odo_cal_b = 1;
  32                 fprintf(stderr, "ODO calibration file not found! \n\n");
  33                 return;
  34         }
  35         char line [15];
  36         float a = 0;
  37         float b = 0;
  38         int num = 0;
  39         while (fgets (line, 15 , file)) {
  40                 a += atof(strtok(line," "));
  41                 fgets (line, 15 , file);
  42                 b += atof(strtok(NULL," "));
  43                 num ++;
  44                 printf("a: %i, b: %i, num: %i\n", a, b, num);
  45         }
  46         fclose (file);
  47         if(a == 0 || b == 0) {
  48                 robot.odo_cal_a = 1;
  49                 robot.odo_cal_b = 1;
  50                 return;
  51         }
  52         robot.odo_cal_a = a / num;
  53         robot.odo_cal_b = b / num;
  54         printf("ODO calibrated value A: %f\n",robot.odo_cal_a);
  55         printf("ODO calibrated value B: %f\n",robot.odo_cal_b);
  56 }
  57
  58 UL_LOG_CUST(ulogd_strategy_odo_calibration); /* Log domain name = ulogd + name of the file */
  59
  60
  61 FSM_STATE(calibrate)
  62 {
  63         switch (FSM_EVENT) {
  64                 case EV_ENTRY:
  65                         start_entry();
  66 //#ifdef COMPETITION
  67                         ul_logmsg("waiting for start\n");
  68                         FSM_TIMER(1000);
  69                         break;
  70 //#endif
  71                 case EV_START:
  72                         start_go();
  73                         robot.use_back_bumpers = false;
  74                         robot.odo_cal_a = 1;
  75                         robot.odo_cal_b = 1;
  76                         FSM_TRANSITION(go_back_for_cal);
  77                         break;
  78                 case EV_TIMER:
  79                         FSM_TIMER(1000);
  80                         start_timer();
  81                         break;
  82                 case EV_EXIT:
  83                         start_exit();
  84                         break;
  85                 case EV_SWITCH_STRATEGY:
  86                         FSM_TRANSITION(central_buillon_wait_for_start);
  87                         break;
  88                 default:;
  89         }
  90 }
  91
  92 /* State for odometry calibration */
  93
  94 FSM_STATE(go_back_for_cal)
  95 {
  96         double x1 = 0, y1 = 0;
  97         char buffer [20];
  98         switch (FSM_EVENT) {
  99                 case EV_ENTRY:
 100                         robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M + 1.0,
 101                                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
 102                                                 0);
 103                         robot_move_by(-1.1, NO_TURN(), &tcVerySlow);
 104                         FSM_TIMER(2000);
 105                         break;
 106                 case EV_MOTION_DONE:
 107                         ROBOT_LOCK(est_pos_odo);
 108                         robot.odo_cal_a = -1.0/robot.odo_distance_a;
 109                         robot.odo_cal_b = -1.0/robot.odo_distance_b;
 110                         x1 = robot.odo_distance_a;
 111                         y1 = robot.odo_distance_b;
 112                         ROBOT_UNLOCK(est_pos_odo);
 113                         if(x1 != 0 || y1 != 0) {
 114                                 printf("Distance for calibration: \n");
 115                                 printf("Left%f\n", x1);
 116                                 printf("Right%f\n", y1);
 117                                 FILE * file;
 118                                 file = fopen ("odometry_cal_data","w");
 119                                 sprintf(buffer, "%4.4f", -1/x1);
 120                                 fputs (buffer,file);
 121                                 fputs (" ", file);
 122                                 sprintf(buffer, "%4.4f", -1/y1);
 123                                 fputs (buffer,file);
 124                                 fputs ("\n", file);
 125                                 fclose(file);
 126                         }
 127                         FSM_TRANSITION(central_buillon_wait_for_start);
 128                         break;
 129                 case EV_TIMER:
 130                         ROBOT_LOCK(est_pos_odo);
 131                         if(x1 == robot.odo_distance_a){
 132                                 x1 = robot.odo_distance_a;
 133                                 y1 = robot.odo_distance_b;
 134                                 //robot_stop();
 135                                 FSM_SIGNAL(MOTION,EV_MOVE_STOP, NULL);
 136                                 FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 137                         } else {
 138                                 FSM_TIMER(10);
 139                         }
 140                         ROBOT_UNLOCK(est_pos_odo);
 141                         break;
 142                 default:
 143                         break;
 144         }
 145 }
 146