src/robofsm/strategy_homologation.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 UL_LOG_CUST(ulogd_strategy_homologation); /* Log domain name = ulogd + name of the file */
  23
  24
  25 FSM_STATE(homologation_wait_for_start)
  26 {
  27         switch (FSM_EVENT) {
  28                 case EV_ENTRY:
  29                         start_entry();
  30 //#ifdef COMPETITION
  31                         ul_logmsg("waiting for start\n");
  32                         FSM_TIMER(1000);
  33                         break;
  34 //#endif
  35                 case EV_START:
  36                         start_go();
  37                         robot_calibrate_odometry();
  38                         FSM_TRANSITION(homologation_approach_buillon);
  39                         break;
  40                 case EV_TIMER:
  41                         FSM_TIMER(1000);
  42                         start_timer();
  43                         break;
  44                 case EV_EXIT:
  45                         start_exit();
  46                         break;
  47                 case EV_SWITCH_STRATEGY:
  48                         FSM_TRANSITION(calibrate);
  49                         break;
  50                 case EV_RETURN:
  51                         break;
  52                 default:;
  53         }
  54 }
  55
  56 FSM_STATE(homologation_approach_buillon)
  57 {
  58         switch(FSM_EVENT) {
  59                 case EV_ENTRY:
  60                         //robot.use_back_bumpers = false;
  61                         robot_trajectory_new(&tcSlow); // new trajectory
  62                         robot_trajectory_add_point_trans(
  63                                 0.65,
  64                                 PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
  65                         robot_trajectory_add_point_trans(
  66                                 0.65,
  67                                 1.3);
  68                         robot_trajectory_add_final_point_trans(
  69                                 0.5,
  70                                 1.1,
  71                                 NO_TURN());
  72                         break;
  73                 case EV_MOTION_DONE:
  74                 case EV_TIMER:
  75                         //robot.use_back_bumpers = true;
  76                         FSM_TRANSITION(homologation_push_bottle);
  77                         break;
  78                 default:
  79                         break;
  80         }
  81 }
  82
  83 FSM_STATE(homologation_push_bottle)
  84 {
  85         switch(FSM_EVENT) {
  86                 case EV_ENTRY:
  87                         robot_trajectory_new(&tcSlow); // new trajectory
  88                         robot_trajectory_add_point_trans(
  89                                 0.64,
  90                                 0.7);
  91                         robot_trajectory_add_final_point_trans(
  92                                 0.64 + 0.08,
  93                                 ROBOT_AXIS_TO_FRONT_M + 0.08,
  94                                 ARRIVE_FROM(DEG2RAD(270), 0.10));
  95                         break;
  96                 case EV_MOTION_DONE:
  97                         break;
  98                 default:
  99                         break;
 100         }
 101 }