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 < (-70.0 / 180.0 * M_PI)) || ((beta > (70.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                         DBG_PRINT_EVENT("recognition");
  89                         act_camera_on();
  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         double alpha, minimum;
 114         double x, y, phi;
 115
 116         switch(FSM_EVENT) {
 117                 case EV_ENTRY:
 118                         DBG_PRINT_EVENT("turn");
 119                         enable_obstacle_avoidance(false);
 120                         get_hokuyo_min(&alpha, &minimum);
 121                         robot_get_est_pos(&x, &y, &phi);
 122                         robot_move_by(0, TURN(alpha+phi), &tcVerySlow);
 123                         break;
 124                 case EV_TIMER:
 125                         break;
 126                 case EV_MOTION_DONE:
 127                         FSM_TRANSITION(get_target_touch);
 128                         break;
 129                 case EV_MOTION_ERROR:
 130                         enable_obstacle_avoidance(true);
 131                         SUBFSM_RET(NULL);
 132                         break;
 133                 case EV_EXIT:
 134                         break;
 135         }
 136 }
 137
 138 FSM_STATE(get_target_touch)
 139 {
 140         double alpha, minimum;
 141         double x, y, phi;
 142
 143         switch(FSM_EVENT) {
 144                 case EV_ENTRY:
 145                         DBG_PRINT_EVENT("touch");
 146                         get_hokuyo_min(&alpha, &minimum);
 147                         robot_move_by(minimum, NO_TURN(), &tcVerySlow);
 148                         break;
 149                 case EV_TIMER:
 150                         break;
 151                 case EV_MOTION_DONE:
 152                         FSM_TRANSITION(get_target_load);
 153                         break;
 154                 case EV_MOTION_ERROR:
 155                         enable_obstacle_avoidance(true);
 156                         SUBFSM_RET(NULL);
 157                         break;
 158                 case EV_EXIT:
 159                         break;
 160         }
 161 }
 162
 163 FSM_STATE(get_target_load)
 164 {
 165         static int direction = 0;
 166
 167         switch(FSM_EVENT) {
 168                 case EV_ENTRY:
 169                         direction = 0;
 170                         act_magnet(1);
 171                         act_crane(CRANE_DOWN);
 172                         break;
 173                 case EV_TIMER:
 174                         robot.target_loaded = true;
 175                         FSM_TRANSITION(get_target_back);
 176                         break;
 177                 case EV_CRANE_DONE:
 178                         if (direction == 0) {
 179                                 direction = 1;
 180                                 act_crane(CRANE_UP);
 181                                 FSM_TIMER(2000);
 182                         }
 183                         break;
 184                 case EV_EXIT:
 185                         break;
 186         }
 187 }
 188
 189 FSM_STATE(get_target_back)
 190 {
 191         switch(FSM_EVENT) {
 192                 case EV_ENTRY:
 193                         robot_move_by(-0.5, NO_TURN(), &tcVerySlow);
 194                         break;
 195                 case EV_TIMER:
 196                         break;
 197                 case EV_CRANE_DONE:
 198                         break;
 199                 case EV_MOTION_DONE:
 200                 case EV_MOTION_ERROR:
 201                         SUBFSM_RET(NULL);
 202                         break;
 203                 case EV_EXIT:
 204                         enable_obstacle_avoidance(true);
 205                         break;
 206         }
 207 }
 208
 209 /*
 210 FSM_STATE()
 211 {
 212         switch(FSM_EVENT) {
 213                 case EV_ENTRY:
 214                         break;
 215                 case EV_START:
 216                 case EV_TIMER:
 217                 case EV_RETURN:
 218                 case EV_CRANE_DONE:
 219                 case EV_MOTION_DONE:
 220                 case EV_MOTION_ERROR:
 221                 case EV_SWITCH_STRATEGY:
 222                         DBG_PRINT_EVENT("unhandled event");
 223                 case EV_EXIT:
 224                         break;
 225         }
 226 }
 227 */