src/robofsm/common-states.cc

   1 #define FSM_MAIN
   2 #include <robot.h>
   3 #include <fsm.h>
   4 #include <unistd.h>
   5 #include <math.h>
   6 #include <movehelper.h>
   7 #include <map.h>
   8 #include <sharp.h>
   9 #include <robomath.h>
  10 #include <string.h>
  11 #include <robodim.h>
  12 #include <error.h>
  13 #include <trgen.h>
  14 #include <stdbool.h>
  15 #include <ul_log.h>
  16 #include <shape_detect.h>
  17
  18 #include "robodata.h"
  19 #include "actuators.h"
  20 #include "match-timing.h"
  21 #include "common-states.h"
  22 #include "sub-states.h"
  23
  24 UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
  25
  26 /************************************************************************
  27  * Functions used in and called from all the (almost identical)
  28  * "wait for start" states in particular strategies.
  29  ************************************************************************/
  30
  31 #undef DBG_FSM_STATE
  32 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  33                                                                    fsm->debug_name, robot_current_time(), \
  34                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  35
  36 /************************************************************************
  37  * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  38  ************************************************************************/
  39
  40 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
  41
  42 /**
  43  * Safe distance for target recognition
  44  */
  45 const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
  46
  47 void set_initial_position()
  48 {
  49         robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
  50 }
  51
  52 void actuators_home()
  53 {
  54         ;
  55 }
  56
  57 /* Check if the new point is within the playground scene */
  58 bool goal_is_in_playground(double goalx, double goaly)
  59 {
  60         if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
  61                 return false;
  62         else
  63                 return true;
  64 }
  65
  66 /* Check if the new point is close to the robot */
  67 bool close_goal(double goalx, double goaly)
  68 {
  69         const double close = 0.5;
  70         double x, y, phi;
  71         robot_get_est_pos(&x, &y, &phi);
  72
  73         if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
  74                 return true;
  75         else
  76                 return false;
  77 }
  78
  79 /**
  80  * FSM state for neighborhood observation.
  81  *
  82  * Detect targets using shape_detect.
  83  * If no target detected, turn 120deg and try again.
  84  * Scan all 360deg and then go back to move_around state.
  85  *
  86  * If target detected, go to approach_target state.
  87  */
  88 FSM_STATE(survey)
  89 {
  90         static char turn_cntr = 0;
  91         double x, y, phi;
  92
  93         switch(FSM_EVENT) {
  94                 case EV_ENTRY:
  95                         DBG_PRINT_EVENT("survey");
  96                         // no target detected in this heading, turn 120°
  97                         robot_get_est_pos(&x, &y, &phi);
  98                         robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
  99                         turn_cntr++;
 100                         DBG_PRINT_EVENT("no target");
 101                         break;
 102                 case EV_TIMER:
 103                         if (turn_cntr > 2) {
 104                                 FSM_TRANSITION(move_around);
 105                         } else {
 106                                 FSM_TRANSITION(survey);
 107                         }
 108                         break;
 109                 case EV_MOTION_DONE:
 110                         FSM_TIMER(1000);
 111                         break;
 112                 case EV_MOTION_ERROR:
 113                         FSM_TRANSITION(move_around);
 114                         break;
 115                 case EV_EXIT:
 116                         turn_cntr = 0;
 117                         break;
 118         }
 119 }
 120
 121 FSM_STATE(move_around)
 122 {
 123         double goalx, goaly;
 124
 125         switch (FSM_EVENT) {
 126                 case EV_ENTRY:
 127                         do {
 128                                 goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
 129                                 goaly = ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
 130                         } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 131
 132                         robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
 133                         DBG_PRINT_EVENT("new survey point");
 134                         break;
 135                 case EV_MOTION_ERROR:
 136                         DBG_PRINT_EVENT("can not access survey point");
 137                 case EV_MOTION_DONE:
 138                         FSM_TRANSITION(survey);
 139                         break;
 140                 case EV_TIMER:
 141                         break;
 142                 case EV_EXIT:
 143                         break;
 144         }
 145 }
 146
 147 FSM_STATE(go_home)
 148 {
 149         switch (FSM_EVENT) {
 150                 case EV_ENTRY:
 151                         DBG_PRINT_EVENT("homing");
 152                         robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcFast);
 153                         break;
 154                 case EV_MOTION_ERROR:
 155                         DBG_PRINT_EVENT("ERROR: home position is not reachable!");
 156                         FSM_TIMER(1000);
 157                         break;
 158                 case EV_TIMER:
 159                         FSM_TRANSITION(go_home);
 160                         break;
 161                 case EV_MOTION_DONE:
 162                 case EV_EXIT:
 163                         DBG_PRINT_EVENT("Mission completed!");
 164                         robot_exit();
 165                         break;
 166         }
 167 }
 168
 169 /*
 170 FSM_STATE()
 171 {
 172         switch(FSM_EVENT) {
 173                 case EV_ENTRY:
 174                         break;
 175                 case EV_START:
 176                 case EV_TIMER:
 177                 case EV_RETURN:
 178                 case EV_CRANE_DONE:
 179                 case EV_MOTION_DONE:
 180                 case EV_MOTION_ERROR:
 181                 case EV_SWITCH_STRATEGY:
 182                         DBG_PRINT_EVENT("unhandled event");
 183                 case EV_EXIT:
 184                         break;
 185         }
 186 }
 187 */