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 enable_obstacle_avoidance(bool enable)
  78 {
  79         robot.obstacle_avoidance_enabled = enable;
  80         robot.ignore_hokuyo = !enable;
  81         robot.check_turn_safety = enable;
  82 }
  83
  84 FSM_STATE(recognize)
  85 {
  86         switch(FSM_EVENT) {
  87                 case EV_ENTRY:
  88                         act_camera_recognize(robot.target_color);
  89                         DBG_PRINT_EVENT("recognition");
  90                         FSM_TIMER(1000);
  91                         break;
  92                 case EV_TIMER:
  93                         FSM_TIMER(1000);
  94                         break;
  95                 case EV_CAMERA_DONE:
  96                         act_camera_off();
  97                         if (robot.target_valid) {
  98                                 DBG_PRINT_EVENT("camera: Target valid!");
  99                                 FSM_TRANSITION(get_target_turn);
 100                         } else {
 101                                 DBG_PRINT_EVENT("camera: Target not valid!");
 102                                 robot.target_loaded = false;
 103                                 SUBFSM_RET(NULL);
 104                         }
 105                         break;
 106                 case EV_EXIT:
 107                         break;
 108         }
 109 }
 110
 111 FSM_STATE(get_target_turn)
 112 {
 113         const double close = 0.05;
 114         double alpha, minimum;
 115         double x, y, phi;
 116
 117         switch(FSM_EVENT) {
 118                 case EV_ENTRY:
 119                         DBG_PRINT_EVENT("turn");
 120                         enable_obstacle_avoidance(false);
 121                         get_hokuyo_min(&alpha, &minimum);
 122                         robot_get_est_pos(&x, &y, &phi);
 123                         robot_move_by(0, TURN(alpha+phi), &tcVerySlow);
 124                         break;
 125                 case EV_TIMER:
 126                         break;
 127                 case EV_MOTION_DONE:
 128                         get_hokuyo_min(&alpha, &minimum);
 129                         if (minimum < close) {
 130                                 FSM_TRANSITION(get_target_load);
 131                         } else {
 132                                 FSM_TRANSITION(get_target_touch);
 133                         }
 134                         break;
 135                 case EV_MOTION_ERROR:
 136                         enable_obstacle_avoidance(true);
 137                         SUBFSM_RET(NULL);
 138                         break;
 139                 case EV_EXIT:
 140                         break;
 141         }
 142 }
 143
 144 FSM_STATE(get_target_touch)
 145 {
 146         const double step = 0.02;
 147         double x, y, phi;
 148
 149         switch(FSM_EVENT) {
 150                 case EV_ENTRY:
 151                         DBG_PRINT_EVENT("touch");
 152                         robot_move_by(step, NO_TURN(), &tcVerySlow);
 153                         break;
 154                 case EV_TIMER:
 155                         break;
 156                 case EV_MOTION_DONE:
 157                         FSM_TRANSITION(get_target_turn);
 158                         break;
 159                 case EV_MOTION_ERROR:
 160                         enable_obstacle_avoidance(true);
 161                         SUBFSM_RET(NULL);
 162                         break;
 163                 case EV_EXIT:
 164                         break;
 165         }
 166 }
 167
 168 FSM_STATE(get_target_load)
 169 {
 170         static int direction = 0;
 171
 172         switch(FSM_EVENT) {
 173                 case EV_ENTRY:
 174                         direction = 0;
 175                         act_jaws(OPEN);
 176                         break;
 177                 case EV_TIMER:
 178                         act_jaws(CATCH);
 179                         robot.target_loaded = true;
 180                         FSM_TRANSITION(get_target_back);
 181                         break;
 182                 case EV_JAWS_DONE:
 183                         if (direction == 0) {
 184                                 direction = 1;
 185                                 FSM_TIMER(2000);
 186                         }
 187                         break;
 188                 case EV_EXIT:
 189                         break;
 190         }
 191 }
 192
 193 FSM_STATE(get_target_back)
 194 {
 195         switch(FSM_EVENT) {
 196                 case EV_ENTRY:
 197                         robot_move_by(-0.5, NO_TURN(), &tcVerySlow);
 198                         break;
 199                 case EV_TIMER:
 200                         break;
 201                 case EV_JAWS_DONE:
 202                         break;
 203                 case EV_MOTION_DONE:
 204                 case EV_MOTION_ERROR:
 205                         SUBFSM_RET(NULL);
 206                         break;
 207                 case EV_EXIT:
 208                         enable_obstacle_avoidance(true);
 209                         break;
 210         }
 211 }
 212
 213 /*
 214 FSM_STATE()
 215 {
 216         switch(FSM_EVENT) {
 217                 case EV_ENTRY:
 218                         break;
 219                 case EV_START:
 220                 case EV_TIMER:
 221                 case EV_RETURN:
 222                 case EV_CRANE_DONE:
 223                 case EV_MOTION_DONE:
 224                 case EV_MOTION_ERROR:
 225                 case EV_SWITCH_STRATEGY:
 226                         DBG_PRINT_EVENT("unhandled event");
 227                 case EV_EXIT:
 228                         break;
 229         }
 230 }
 231 */