src/robofsm/strategy_get_central_buillon.cc

   1 #include "common-states.h"
   2 #include "robot.h"
   3 #include <ul_log.h>
   4
   5
   6 UL_LOG_CUST(ulogd_strategy_central_buillon_wait_for_start); /* Log domain name = ulogd + name of the file */
   7 static FSM_STATE_DECL(pick_central_buillon);
   8
   9
  10 FSM_STATE(central_buillon_wait_for_start)
  11 {
  12         switch (FSM_EVENT) {
  13                 case EV_ENTRY:
  14                         start_entry();
  15 //#ifdef COMPETITION
  16                         ul_logmsg("waiting for start\n");
  17                         FSM_TIMER(1000);
  18                         break;
  19 //#endif
  20                 case EV_START:
  21                         start_go();
  22                         robot_calibrate_odometry();
  23                         FSM_TRANSITION(pick_central_buillon);
  24                         break;
  25                 case EV_TIMER:
  26                         FSM_TIMER(1000);
  27                         start_timer();
  28                         break;
  29                 case EV_EXIT:
  30                         start_exit();
  31                         break;
  32                 case EV_SWITCH_STRATEGY:
  33                         FSM_TRANSITION(homologation_wait_for_start);
  34                         break;
  35                 default:;
  36         }
  37 }
  38 #undef FSM_STATE_VISIBILITY
  39 #define FSM_STATE_VISIBILITY static
  40
  41 FSM_STATE_DECL(bottle_bw);
  42 FSM_STATE_DECL(pick_buillon_from_totem1);
  43 FSM_STATE_DECL(pick_buillon_from_totem2);
  44 FSM_STATE_DECL(push_second_bottle);
  45
  46
  47 FSM_STATE(pick_central_buillon)
  48 {
  49         switch (FSM_EVENT) {
  50                 case EV_ENTRY:
  51                         SUBFSM_TRANSITION(approach_central_buillon,NULL);
  52                         break;
  53                 case EV_RETURN:
  54                         FSM_TRANSITION(bottle_bw);
  55                         break;
  56                 default:
  57                         break;
  58         }
  59 }
  60
  61 FSM_STATE(bottle_bw)
  62 {
  63         switch (FSM_EVENT) {
  64                 case EV_ENTRY:
  65                         SUBFSM_TRANSITION(push_bottle_bw,NULL);
  66                         up = false;
  67                         break;
  68                 case EV_RETURN:
  69                         FSM_TRANSITION(pick_buillon_from_totem1);
  70                         break;
  71                 default:
  72                         break;
  73         }
  74 }
  75
  76 FSM_STATE(pick_buillon_from_totem1)
  77 {
  78         switch (FSM_EVENT) {
  79                 case EV_ENTRY:
  80                         totem_x = 1.1;
  81                         totem_y = 0.875;
  82                         SUBFSM_TRANSITION(leave_home,NULL);
  83                         break;
  84                 case EV_RETURN:
  85                         FSM_TRANSITION(push_second_bottle);
  86                         break;
  87                 default:
  88                         break;
  89         }
  90 }
  91
  92 FSM_STATE(pick_buillon_from_totem2)
  93 {
  94         switch (FSM_EVENT) {
  95                 case EV_ENTRY:
  96                         up = true;
  97                         totem_x = 1.1;
  98                         totem_y = 1.125;
  99                         SUBFSM_TRANSITION(leave_home,NULL);
 100                         break;
 101                 default:
 102                         break;
 103         }
 104 }
 105
 106 FSM_STATE(push_second_bottle)
 107 {
 108         switch (FSM_EVENT) {
 109                 case EV_ENTRY:
 110                         SUBFSM_TRANSITION(go_back_from_home, NULL);
 111                         break;
 112                 case EV_RETURN:
 113                         break;
 114         }
 115 }