src/robofsm/robot_orte.c

   1 /**
   2  * @file robot_orte.c
   3  * @date 2008-2010
   4  *
   5  * @brief Module providing communication through ORTE
   6  */
   7
   8 /*
   9  * robot_orte.c                 08/04/21
  10  *
  11  * Robot's orte stuffs.
  12  *
  13  * Copyright: (c) 2008-2010 CTU Dragons
  14  *            CTU FEE - Department of Control Engineering
  15  * License: GNU GPL v.2
  16  */
  17
  18 #include <orte.h>
  19 #include <roboorte_robottype.h>
  20 #include "robodata.h"
  21 #include <robot.h>
  22 #include <movehelper.h>
  23 #include <math.h>
  24 #include <robomath.h>
  25 #include "map_handling.h"
  26 #include <string.h>
  27 #include <can_msg_def.h>
  28 #include <actuators.h>
  29 #include <ul_log.h>
  30
  31 UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
  32
  33 #ifdef ORTE_DEBUG
  34    #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
  35 #else
  36    #define DBG(format, ...)
  37 #endif
  38
  39 /*****FIXME:*****/
  40 extern sem_t measurement_received;
  41
  42 /**
  43  * Check rrobot component status
  44  * @return zero if system OK, else 1
  45  */
  46 int check_prestart_status()
  47 {
  48         if ((robot.status[COMPONENT_MOTOR] == STATUS_FAILED) || (robot.status[COMPONENT_MOTOR] == STATUS_WARNING)) {
  49                 return 1;
  50         } else if (robot.status[COMPONENT_ODOMETRY] == STATUS_FAILED) {
  51                 return 1;
  52         } else if ((robot.status[COMPONENT_CAMERA] == STATUS_FAILED) || (robot.status[COMPONENT_CAMERA] == STATUS_WARNING)) {
  53                 return 1;
  54         } else if (robot.status[COMPONENT_POWER] == STATUS_FAILED) {
  55                 return 1;
  56         } else if (robot.status[COMPONENT_HOKUYO] == STATUS_FAILED) {
  57                 return 1;
  58         } else if (robot.status[COMPONENT_CRANE] == STATUS_FAILED) {
  59                 return 1;
  60         } else if (robot.status[COMPONENT_START] == STATUS_FAILED || robot.status[COMPONENT_START] == STATUS_WARNING) {
  61                 return 1;
  62         } else {
  63                 return 0;
  64         }
  65 }
  66 /* ----------------------------------------------------------------------
  67  * PUBLISHER CALLBACKS - GENERIC
  68  * ---------------------------------------------------------------------- */
  69
  70 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  71                         void *sendCallBackParam)
  72 {
  73         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  74
  75         ROBOT_LOCK(ref_pos);
  76         *instance = robot.ref_pos;
  77         ROBOT_UNLOCK(ref_pos);
  78 }
  79
  80 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
  81                         void *sendCallBackParam)
  82 {
  83         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  84
  85         ROBOT_LOCK(est_pos_odo);
  86         *instance = robot.est_pos_odo;
  87         ROBOT_UNLOCK(est_pos_odo);
  88 }
  89
  90 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
  91                         void *sendCallBackParam)
  92 {
  93         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  94
  95         ROBOT_LOCK(est_pos_indep_odo);
  96         *instance = robot.est_pos_indep_odo;
  97         ROBOT_UNLOCK(est_pos_indep_odo);
  98 }
  99
 100 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
 101                         void *sendCallBackParam)
 102 {
 103         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
 104
 105         if (check_prestart_status()) {
 106                 /* if any component not ready, send component error in best position
 107                 for display to show APP yellow */
 108                 instance->x = COMPONENT_ERR;
 109                 instance->y = COMPONENT_ERR;
 110                 instance->phi = COMPONENT_ERR;
 111         } else {
 112                 robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
 113         }
 114 }
 115
 116 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
 117                         void *sendCallBackParam)
 118 {
 119 }
 120
 121 /* ----------------------------------------------------------------------
 122  * SUBSCRIBER CALLBACKS - GENERIC
 123  * ---------------------------------------------------------------------- */
 124 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
 125                         void *recvCallBackParam)
 126 {
 127         struct odo_data_type *instance = (struct odo_data_type *)vinstance;
 128         double dleft, dright, dtang, dphi;
 129         static bool first_run = true;
 130         /* spocitat prevodovy pomer */
 131         const double n = (double)(1.0 / 1.0);
 132
 133         /* vzdalenost na pulz IRC */
 134         const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
 135
 136         switch (info->status) {
 137                 case NEW_DATA:
 138                         if (first_run) {
 139                                 ROBOT_LOCK(odo_data);
 140                                 robot.odo_data = *instance;
 141                                 ROBOT_UNLOCK(odo_data);
 142                                 first_run = false;
 143                                 break;
 144                         }
 145
 146                         dleft = ((robot.odo_data.left - instance->left) >> 8) * c;      // TODO >> 8 ?
 147                         dright = ((instance->right - robot.odo_data.right) >> 8) * c;
 148
 149                         dtang = (dleft + dright) / 2.0;
 150                         dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 151
 152                         ROBOT_LOCK(est_pos_indep_odo);
 153                         double a = robot.est_pos_indep_odo.phi;
 154                         robot.est_pos_indep_odo.x += dtang*cos(a);
 155                         robot.est_pos_indep_odo.y += dtang*sin(a);
 156                         robot.est_pos_indep_odo.phi += dphi;
 157                         ROBOT_UNLOCK(est_pos_indep_odo);
 158
 159                         ROBOT_LOCK(odo_data);
 160                         robot.odo_data = *instance;
 161                         ROBOT_UNLOCK(odo_data);
 162
 163                         robot.indep_odometry_works = true;
 164
 165                         /* wake up motion-control/thread_trajectory_follower */
 166                         sem_post(&measurement_received);
 167
 168                         //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
 169                         break;
 170                 case DEADLINE:
 171                         robot.indep_odometry_works = false;
 172                         //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
 173                         DBG("ORTE deadline occurred - odo_data receive\n");
 174                         break;
 175         }
 176 }
 177
 178 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 179                         void *recvCallBackParam)
 180 {
 181         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
 182         double dleft, dright, dtang, dphi;
 183         static bool first_run = true;
 184         /* spocitat prevodovy pomer */
 185         const double n = (double)(28.0 / 1.0);
 186
 187         /* vzdalenost na pulz IRC */
 188         const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
 189
 190         switch (info->status) {
 191                 case NEW_DATA:
 192                         if (first_run) {
 193                                 ROBOT_LOCK(motion_irc);
 194                                 robot.motion_irc = *instance;
 195                                 ROBOT_UNLOCK(motion_irc);
 196                                 first_run = false;
 197                                 break;
 198                         }
 199
 200                         dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
 201                         dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
 202
 203                         dtang = (dleft + dright) / 2.0;
 204                         dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 205
 206                         ROBOT_LOCK(est_pos_odo);
 207                         double a = robot.est_pos_odo.phi;
 208                         robot.est_pos_odo.x += dtang*cos(a);
 209                         robot.est_pos_odo.y += dtang*sin(a);
 210                         robot.est_pos_odo.phi += dphi;
 211                         ROBOT_UNLOCK(est_pos_odo);
 212
 213                         /* locking should not be needed, but... */
 214                         ROBOT_LOCK(motion_irc);
 215                         robot.motion_irc = *instance;
 216                         robot.motion_irc_received = 1;
 217                         ROBOT_UNLOCK(motion_irc);
 218
 219                         robot.odometry_works = true;
 220
 221                         robot.status[COMPONENT_MOTOR] = STATUS_OK;
 222                         break;
 223                 case DEADLINE:
 224                         robot.odometry_works = false;
 225                         robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
 226                         DBG("ORTE deadline occurred - motion_irc receive\n");
 227                         break;
 228         }
 229 }
 230
 231 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 232                         void *recvCallBackParam)
 233 {
 234         switch (info->status) {
 235                 case NEW_DATA:
 236                         break;
 237                 case DEADLINE:
 238                         DBG("ORTE deadline occurred - motion_speed receive\n");
 239                         break;
 240         }
 241 }
 242
 243 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 244                         void *recvCallBackParam)
 245 {
 246         switch (info->status) {
 247                 case NEW_DATA:
 248                         break;
 249                 case DEADLINE:
 250                         DBG("ORTE deadline occurred - motion_status receive\n");
 251                         break;
 252         }
 253 }
 254
 255 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 256                         void *recvCallBackParam)
 257 {
 258         switch (info->status) {
 259                 case NEW_DATA:
 260                         robot.status[COMPONENT_POWER]=STATUS_OK;
 261                         break;
 262                 case DEADLINE:
 263                         robot.status[COMPONENT_POWER]=STATUS_FAILED;
 264                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 265                         break;
 266         }
 267 }
 268
 269 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 270                         void *recvCallBackParam)
 271 {
 272         switch (info->status) {
 273                 case NEW_DATA:
 274                         break;
 275                 case DEADLINE:
 276                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 277                         break;
 278         }
 279 }
 280
 281 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 282                         void *recvCallBackParam)
 283 {
 284         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 285         static struct robot_cmd_type last_instance;
 286
 287         switch (info->status) {
 288                 case NEW_DATA:
 289                         /* Stupid way of controlling the robot by
 290                          * pluggin in and out start connector. */
 291                         switch (robot.start_state) {
 292                                 case POWER_ON:
 293                                         if (instance->start_condition == START_PLUGGED_IN) {
 294                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 295                                                 robot.start_state = START_PLUGGED_IN;
 296                                                 ul_logmsg("START_PLUGGED_IN\n");
 297                                         }
 298                                         break;
 299
 300                                 case START_PLUGGED_IN:
 301                                         robot.status[COMPONENT_START] = STATUS_OK;
 302                                         /* Competition starts when plugged out */
 303                                         if (instance->start_condition == COMPETITION_STARTED) {
 304                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 305                                                 robot.start_state = COMPETITION_STARTED;
 306                                                 ul_logmsg("STARTED\n");
 307                                         }
 308                                         break;
 309
 310                                 case COMPETITION_STARTED: {
 311                                         /* Subsequent plug-in stops the robot */
 312                                         static int num = 0;
 313                                         if (instance->start_condition == START_PLUGGED_IN) {
 314                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 315                                                 if (++num == 10)
 316                                                         robot_exit();
 317                                         }
 318                                         break;
 319                                 }
 320                         }
 321                         last_instance = *instance;
 322                         break;
 323                 case DEADLINE:
 324                         robot.status[COMPONENT_START] = STATUS_FAILED;
 325                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 326                         break;
 327         }
 328 }
 329
 330 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 331                         void *recvCallBackParam)
 332 {
 333         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 334
 335         switch (info->status) {
 336                 case NEW_DATA: {
 337                         ROBOT_LOCK(hokuyo);
 338                         robot.hokuyo = *instance;
 339                         robot.status[COMPONENT_HOKUYO] = STATUS_OK;
 340                         ROBOT_UNLOCK(hokuyo);
 341                         if(!robot.ignore_hokuyo)
 342                         {
 343                                 update_map_hokuyo(instance);
 344                         }
 345                         break;
 346                 }
 347                 case DEADLINE:
 348                         robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
 349                         //system("killall -9 hokuyo");
 350                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 351                         break;
 352         }
 353 }
 354
 355 // FIXME use this for recieve data from target recognition
 356 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 357                         void *recvCallBackParam)
 358 {
 359         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 360         static bool last_response = false;
 361
 362         switch (info->status) {
 363                 case NEW_DATA: {
 364                         if (instance->error) {
 365                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 366                         } else if (instance->data_valid && instance->data_valid != last_response) {
 367                                 ROBOT_LOCK(camera_result);
 368                                 robot.target_valid = instance->target_valid;
 369                                 ROBOT_UNLOCK(camera_result);
 370                                 FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
 371                                 printf("robot_orte: valid: %d\n", instance->target_valid);
 372
 373                                 robot.status[COMPONENT_CAMERA] = STATUS_OK;
 374                         }
 375                         last_response = instance->data_valid;
 376                         break;
 377                 }
 378                 case DEADLINE:
 379                         if (robot.orte.camera_control.on) {
 380                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 381                                 //system("killall -9 rozpuk");
 382                         }
 383
 384                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 385                         break;
 386         }
 387 }
 388
 389 void rcv_crane_status_cb(const ORTERecvInfo *info, void *vinstance,
 390                             void *recvCallBackParam)
 391 {
 392         struct crane_status_type *instance = (struct crane_status_type *)vinstance;
 393         static int last_response = 0;
 394         switch (info->status) {
 395                 case NEW_DATA:
 396                         // new data arrived and requested position equals actual position
 397                         if (instance->flags == 0)
 398                                 robot.status[COMPONENT_CRANE]=STATUS_OK;
 399                         else
 400                                 robot.status[COMPONENT_CRANE]=STATUS_WARNING;
 401
 402                         if (instance->response != last_response &&
 403                             instance->response == act_crane_get_last_reqest())
 404                                 FSM_SIGNAL(MAIN, EV_CRANE_DONE, NULL);
 405                         last_response = instance->response;
 406                         break;
 407                 case DEADLINE:
 408                         robot.status[COMPONENT_CRANE]=STATUS_FAILED;
 409                         DBG("ORTE deadline occurred - actuator_status receive\n");
 410                         break;
 411         }
 412 }
 413
 414 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
 415                            void *recvCallBackParam)
 416 {
 417         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 418         static bool last_strategy;
 419         switch (info->status) {
 420                 case NEW_DATA:
 421                         if ((!last_strategy && instance->strategy) && (robot.start_state == POWER_ON)) {
 422                                 FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
 423                         }
 424                         last_strategy = instance->strategy;
 425                         break;
 426                 case DEADLINE:
 427                         break;
 428         }
 429 }
 430
 431 void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
 432                            void *recvCallBackParam)
 433 {
 434         struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
 435         static bool last_left, last_right;
 436         switch (info->status) {
 437                 case NEW_DATA:
 438                         if (instance->bumper_rear)
 439                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 440                         if ((!last_left && instance->bumper_left) ||
 441                             (!last_right && instance->bumper_right)) {
 442                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
 443                         }
 444                         last_right = instance->bumper_right;
 445                         last_left = instance->bumper_left;
 446                         break;
 447                 case DEADLINE:
 448                         break;
 449         }
 450 }
 451
 452
 453 #define HIST_CNT 5
 454 #if 0
 455 static int cmp_double(const void *v1, const void *v2)
 456 {
 457         const double *d1 = v1, *const d2 = v2;
 458         if (d1 < d2)
 459                 return -1;
 460         else if (d1 > d2)
 461                 return +1;
 462         else
 463                 return 0;
 464 }
 465 #endif
 466
 467 int robot_init_orte()
 468 {
 469         int rv = 0;
 470
 471         robot.orte.strength = 20;
 472
 473         rv = robottype_roboorte_init(&robot.orte);
 474         if (rv)
 475                 return rv;
 476
 477         /* creating publishers */
 478         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 479         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 480         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 481         robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
 482         robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
 483         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 484
 485         // I didn't know what was the difference between the callback function pointer
 486         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 487         // IS CRUCIAL!
 488         //   - NULL: message is published only when OrtePublicationSend called
 489         //   - pointer to empty function: message is published periodically
 490         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 491         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 492         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 493         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 494         robottype_publisher_crane_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 495         robottype_publisher_magnet_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 496
 497         /* create generic subscribers */
 498         robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
 499         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 500         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 501         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 502         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 503         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 504         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 505         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 506         robottype_subscriber_crane_status_create(&robot.orte, rcv_crane_status_cb, &robot.orte);
 507         robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
 508         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 509
 510         return rv;
 511 }
 512