src/robofsm/sub-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 <robomath.h>
   9 #include <string.h>
  10 #include <robodim.h>
  11 #include <error.h>
  12 #include <trgen.h>
  13 #include <stdbool.h>
  14 #include <ul_log.h>
  15 #include <hokuyo.h>
  16
  17 #include "robodata.h"
  18 #include "actuators.h"
  19 #include "match-timing.h"
  20 #include "common-states.h"
  21 #include "sub-states.h"
  22
  23 UL_LOG_CUST(ulogd_sub_states); /* Log domain name = ulogd + name of the file */
  24
  25 extern struct TrajectoryConstraints tcSlow, tcVerySlow;
  26
  27 /************************************************************************
  28  * Functions used in and called from all the (almost identical)
  29  * "wait for start" states in particular strategies.
  30  ************************************************************************/
  31
  32 #undef DBG_FSM_STATE
  33 #define DBG_FSM_STATE(name)     do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
  34                                                                    fsm->debug_name, robot_current_time(), \
  35                                                                    name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
  36
  37
  38 #define HOK_MIN_MM (ROBOT_AXIS_TO_FRONT_MM - HOKUYO_CENTER_OFFSET_MM)
  39 #define HOK_MIN_M  (HOK_MIN_MM/1000.0)
  40 #define HOK_MAX_MM (1000)
  41
  42 /**
  43  * Count the angle to turn by to see the minimum distance in the robot axis.
  44  * @param alpha poiner to the angle to turn by
  45  * @param minimum pointer to the minimum distance from hokuyo
  46  */
  47 void get_hokuyo_min(double *alpha, double *minimum)
  48 {
  49         double beta;
  50         double min = 1000;
  51         int min_i;
  52
  53         struct hokuyo_scan_type scan = robot.hokuyo;
  54         uint16_t *data = scan.data;
  55
  56         for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
  57                 beta = HOKUYO_INDEX_TO_RAD(i);
  58                 if((beta < (-50.0 / 180.0 * M_PI)) || ((beta > (50.0 / 180.0 * M_PI))))
  59                         continue;
  60
  61                 if(data[i] > HOK_MIN_MM && data[i] < HOK_MAX_MM) {
  62                         if (data[i] < min ) {
  63                                 min = data[i];
  64                                 min_i = i;
  65                         }
  66                        // printf("min: %f, beta: %f\n", min, beta);
  67                 }
  68         }
  69         min /= 1000.0;
  70         beta = HOKUYO_INDEX_TO_RAD(min_i);
  71         *alpha = atan2((min * sin(beta)), (HOKUYO_CENTER_OFFSET_M + min * cos(beta)));
  72         *minimum = min - HOK_MIN_M;
  73
  74         printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
  75 }
  76
  77 void get_camera_ang(double *alpha)
  78 {
  79         *alpha = DEG2RAD(robot.target_ang);
  80 }
  81
  82 void enable_obstacle_avoidance(bool enable)
  83 {
  84         robot.obstacle_avoidance_enabled = enable;
  85         robot.ignore_hokuyo = !enable;
  86         robot.check_turn_safety = enable;
  87 }
  88
  89 FSM_STATE(recognize)
  90 {
  91         static int camera_delay = 0;
  92
  93         switch(FSM_EVENT) {
  94                 case EV_ENTRY:
  95                         act_camera_recognize(robot.target_color);
  96                         DBG_PRINT_EVENT("recognition");
  97                         FSM_TIMER(1000);
  98                         break;
  99                 case EV_TIMER:
 100                         if (++camera_delay > 10) {
 101                                 // waiting for 10 seconds now, return back
 102                                 DBG_PRINT_EVENT("camera: No response!");
 103                                 ROBOT_LOCK(camera_result);
 104                                 robot.target_valid = false;
 105                                 ROBOT_UNLOCK(camera_result);
 106                                 SUBFSM_RET(NULL);
 107                         } else {
 108                                 FSM_TIMER(1000);
 109                         }
 110                         break;
 111                 case EV_CAMERA_DONE:
 112                         act_camera_off();
 113                         if (robot.target_valid) {
 114                                 robot.target_valid = false;
 115                                 DBG_PRINT_EVENT("camera: Target valid!");
 116                                 FSM_TRANSITION(get_target_turn);
 117                         } else {
 118                                 DBG_PRINT_EVENT("camera: Target not valid!");
 119                                 robot.target_loaded = false;
 120                                 SUBFSM_RET(NULL);
 121                         }
 122                         break;
 123                 case EV_EXIT:
 124                         camera_delay = 0;
 125                         break;
 126         }
 127 }
 128
 129 FSM_STATE(get_target_turn)
 130 {
 131         const double close = 0.05;
 132         double alpha, minimum;
 133         double x, y, phi;
 134         static char first = 1;
 135
 136         switch(FSM_EVENT) {
 137                 case EV_ENTRY:
 138                         DBG_PRINT_EVENT("turn");
 139                         //enable_obstacle_avoidance(false);
 140
 141                         if (0) {
 142                                 // use angle from camera on first run
 143                                 get_camera_ang(&alpha);
 144                                 minimum = 1;
 145                                 first = 0;
 146                         } else {
 147                                 // than navigate using hokuyo
 148                                 get_hokuyo_min(&alpha, &minimum);
 149                         }
 150
 151                         robot_get_est_pos(&x, &y, &phi);
 152                         robot_move_by(0, TURN(alpha+phi), &tcSlow);
 153                         break;
 154                 case EV_TIMER:
 155                         break;
 156                 case EV_MOTION_DONE:
 157                         get_hokuyo_min(&alpha, &minimum);
 158                         if (minimum < close) {
 159                                 FSM_TRANSITION(get_target_load);
 160                         } else {
 161                                 FSM_TRANSITION(get_target_touch);
 162                         }
 163                         break;
 164                 case EV_MOTION_ERROR:
 165                         enable_obstacle_avoidance(true);
 166                         SUBFSM_RET(NULL);
 167                         break;
 168                 case EV_EXIT:
 169                         break;
 170         }
 171 }
 172
 173 FSM_STATE(get_target_touch)
 174 {
 175         const double step = 0.02;
 176         double x, y, phi;
 177
 178         switch(FSM_EVENT) {
 179                 case EV_ENTRY:
 180                         DBG_PRINT_EVENT("touch");
 181                         robot_move_by(step, NO_TURN(), &tcSlow);
 182                         break;
 183                 case EV_TIMER:
 184                         break;
 185                 case EV_MOTION_DONE:
 186                         FSM_TRANSITION(get_target_turn);
 187                         break;
 188                 case EV_MOTION_ERROR:
 189                         enable_obstacle_avoidance(true);
 190                         SUBFSM_RET(NULL);
 191                         break;
 192                 case EV_EXIT:
 193                         break;
 194         }
 195 }
 196
 197 FSM_STATE(get_target_load)
 198 {
 199         static char delay = 0;
 200
 201         switch(FSM_EVENT) {
 202                 case EV_ENTRY:
 203                         break;
 204                 case EV_TIMER:
 205                         if (++delay > 3) {
 206                                 // target was not confirmed
 207                                 DBG_PRINT_EVENT("OMRON: Target NOT valid!");
 208                                 robot.target_loaded = false;
 209                                 robot.target_valid  = false;
 210                                 FSM_TRANSITION(get_target_unload);
 211                         } else {
 212                                 FSM_TIMER(1000);
 213                         }
 214                         break;
 215                 case EV_OMRON_DONE:
 216                         // target confirmed as valid by OMRON sensor
 217                         // return to main automaton and transport target to storage
 218                         DBG_PRINT_EVENT("OMRON: Target valid!");
 219                         robot.target_loaded = true;
 220                         robot.target_valid  = true;
 221                         SUBFSM_RET(NULL);
 222                         break;
 223                 case EV_EXIT:
 224                         break;
 225         }
 226 }
 227
 228 /* unload target if not valid = no color match using OMRON */
 229 FSM_STATE(get_target_unload)
 230 {
 231         switch(FSM_EVENT) {
 232                 case EV_ENTRY:
 233                         break;
 234                 case EV_TIMER:
 235                         robot.target_loaded = true;
 236                         FSM_TRANSITION(get_target_back);
 237                         break;
 238
 239                 case EV_EXIT:
 240                         break;
 241         }
 242 }
 243
 244 FSM_STATE(get_target_back)
 245 {
 246         switch(FSM_EVENT) {
 247                 case EV_ENTRY:
 248                         robot_move_by(-0.5, NO_TURN(), &tcVerySlow);
 249                         break;
 250                 case EV_TIMER:
 251                         break;
 252                 case EV_MOTION_DONE:
 253                 case EV_MOTION_ERROR:
 254                         SUBFSM_RET(NULL);
 255                         break;
 256                 case EV_EXIT:
 257                         enable_obstacle_avoidance(true);
 258                         break;
 259         }
 260 }
 261
 262 /*
 263 FSM_STATE()
 264 {
 265         switch(FSM_EVENT) {
 266                 case EV_ENTRY:
 267                         break;
 268                 case EV_START:
 269                 case EV_TIMER:
 270                 case EV_RETURN:
 271                 case EV_CRANE_DONE:
 272                 case EV_MOTION_DONE:
 273                 case EV_MOTION_ERROR:
 274                 case EV_SWITCH_STRATEGY:
 275                         DBG_PRINT_EVENT("unhandled event");
 276                 case EV_EXIT:
 277                         break;
 278         }
 279 }
 280 */