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  * PUBLISHER CALLBACKS - GENERIC
  44  * ---------------------------------------------------------------------- */
  45
  46 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
  47                         void *sendCallBackParam)
  48 {
  49         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  50
  51         ROBOT_LOCK(ref_pos);
  52         *instance = robot.ref_pos;
  53         ROBOT_UNLOCK(ref_pos);
  54 }
  55
  56 void send_est_pos_odo_cb(const ORTESendInfo *info, void *vinstance,
  57                         void *sendCallBackParam)
  58 {
  59         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  60
  61         ROBOT_LOCK(est_pos_odo);
  62         *instance = robot.est_pos_odo;
  63         ROBOT_UNLOCK(est_pos_odo);
  64 }
  65
  66 void send_est_pos_indep_odo_cb(const ORTESendInfo *info, void *vinstance,
  67                         void *sendCallBackParam)
  68 {
  69         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  70
  71         ROBOT_LOCK(est_pos_indep_odo);
  72         *instance = robot.est_pos_indep_odo;
  73         ROBOT_UNLOCK(est_pos_indep_odo);
  74 }
  75
  76 static void send_est_pos_best_cb(const ORTESendInfo *info, void *vinstance,
  77                         void *sendCallBackParam)
  78 {
  79         struct robot_pos_type *instance = (struct robot_pos_type *)vinstance;
  80
  81         robot_get_est_pos(&instance->x, &instance->y, &instance->phi);
  82 }
  83
  84 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
  85                         void *sendCallBackParam)
  86 {
  87 }
  88
  89 /* ----------------------------------------------------------------------
  90  * SUBSCRIBER CALLBACKS - GENERIC
  91  * ---------------------------------------------------------------------- */
  92 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
  93                         void *recvCallBackParam)
  94 {
  95         struct odo_data_type *instance = (struct odo_data_type *)vinstance;
  96         double dleft, dright, dtang, dphi;
  97         static bool first_run = true;
  98         /* spocitat prevodovy pomer */
  99         const double n = (double)(1.0 / 1.0);
 100
 101         /* vzdalenost na pulz IRC */
 102         const double c = (M_PI*2*ODOMETRY_WHEEL_RADIUS_M) / (n * 4*4096.0);
 103
 104         switch (info->status) {
 105                 case NEW_DATA:
 106                         if (first_run) {
 107                                 ROBOT_LOCK(odo_data);
 108                                 robot.odo_data = *instance;
 109                                 ROBOT_UNLOCK(odo_data);
 110                                 first_run = false;
 111                                 break;
 112                         }
 113
 114                         dleft = ((robot.odo_data.left - instance->left) >> 8) * c;      // TODO >> 8 ?
 115                         dright = ((instance->right - robot.odo_data.right) >> 8) * c;
 116
 117                         dtang = (dleft + dright) / 2.0;
 118                         dphi = (dright - dleft) / (2.0*ODOMETRY_ROTATION_RADIUS_M);
 119
 120                         ROBOT_LOCK(est_pos_indep_odo);
 121                         double a = robot.est_pos_indep_odo.phi;
 122                         robot.est_pos_indep_odo.x += dtang*cos(a);
 123                         robot.est_pos_indep_odo.y += dtang*sin(a);
 124                         robot.est_pos_indep_odo.phi += dphi;
 125                         ROBOT_UNLOCK(est_pos_indep_odo);
 126
 127                         ROBOT_LOCK(odo_data);
 128                         robot.odo_data = *instance;
 129                         ROBOT_UNLOCK(odo_data);
 130
 131                         robot.indep_odometry_works = true;
 132
 133                         /* wake up motion-control/thread_trajectory_follower */
 134                         sem_post(&measurement_received);
 135
 136                         //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
 137                         break;
 138                 case DEADLINE:
 139                         robot.indep_odometry_works = false;
 140                         //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
 141                         DBG("ORTE deadline occurred - odo_data receive\n");
 142                         break;
 143         }
 144 }
 145
 146 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
 147                         void *recvCallBackParam)
 148 {
 149         struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
 150         double dleft, dright, dtang, dphi;
 151         static bool first_run = true;
 152         /* spocitat prevodovy pomer */
 153         const double n = (double)(ROBOT_MOTOR_GEARBOX_RATIO / 1.0);
 154
 155         /* vzdalenost na pulz IRC */
 156         const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*ROBOT_MOTOR_IRC_RESOLUTION);
 157         switch (info->status) {
 158                 case NEW_DATA:
 159                         if (first_run) {
 160                                 ROBOT_LOCK(motion_irc);
 161                                 robot.motion_irc = *instance;
 162                                 ROBOT_UNLOCK(motion_irc);
 163                                 first_run = false;
 164                                 break;
 165                         }
 166
 167
 168                         /* FIXME maybe it is not correct to do this nasty hack here (switch dleft and dright),
 169                         what is the right solution?
 170                         This was neccessary to view motor odometry correctly in robomon. */
 171                         dright = ((robot.motion_irc.left - instance->left) >> 8) * c;
 172                         dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
 173
 174                         dtang = (dleft + dright) / 2.0;
 175                         dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
 176
 177                         ROBOT_LOCK(est_pos_odo);
 178                         double a = robot.est_pos_odo.phi;
 179                         robot.est_pos_odo.x += dtang*cos(a);
 180                         robot.est_pos_odo.y += dtang*sin(a);
 181                         robot.est_pos_odo.phi += dphi;
 182                         ROBOT_UNLOCK(est_pos_odo);
 183
 184                         /* locking should not be needed, but... */
 185                         ROBOT_LOCK(motion_irc);
 186                         robot.motion_irc = *instance;
 187                         robot.motion_irc_received = 1;
 188                         ROBOT_UNLOCK(motion_irc);
 189
 190                         robot.odometry_works = true;
 191
 192                         robot.status[COMPONENT_MOTOR] = STATUS_OK;
 193                         break;
 194                 case DEADLINE:
 195                         robot.odometry_works = false;
 196                         robot.status[COMPONENT_MOTOR] = STATUS_FAILED;
 197                         DBG("ORTE deadline occurred - motion_irc receive\n");
 198                         break;
 199         }
 200 }
 201
 202 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 203                         void *recvCallBackParam)
 204 {
 205         switch (info->status) {
 206                 case NEW_DATA:
 207                         break;
 208                 case DEADLINE:
 209                         DBG("ORTE deadline occurred - motion_speed receive\n");
 210                         break;
 211         }
 212 }
 213
 214 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
 215                         void *recvCallBackParam)
 216 {
 217         switch (info->status) {
 218                 case NEW_DATA:
 219                         break;
 220                 case DEADLINE:
 221                         DBG("ORTE deadline occurred - motion_status receive\n");
 222                         break;
 223         }
 224 }
 225
 226 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 227                         void *recvCallBackParam)
 228 {
 229         switch (info->status) {
 230                 case NEW_DATA:
 231                         robot.status[COMPONENT_POWER]=STATUS_OK;
 232                         break;
 233                 case DEADLINE:
 234                         robot.status[COMPONENT_POWER]=STATUS_FAILED;
 235                         DBG("ORTE deadline occurred - pwr_voltage receive\n");
 236                         break;
 237         }
 238 }
 239
 240 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
 241                         void *recvCallBackParam)
 242 {
 243         switch (info->status) {
 244                 case NEW_DATA:
 245                         break;
 246                 case DEADLINE:
 247                         DBG("ORTE deadline occurred - pwr_ctrl receive\n");
 248                         break;
 249         }
 250 }
 251
 252 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 253                         void *recvCallBackParam)
 254 {
 255         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 256         static struct robot_cmd_type last_instance;
 257
 258         switch (info->status) {
 259                 case NEW_DATA:
 260                         /* Stupid way of controlling the robot by
 261                          * pluggin in and out start connector. */
 262                         switch (robot.start_state) {
 263                                 case POWER_ON:
 264                                         if (!instance->start_condition) {
 265                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 266                                                 robot.start_state = START_PLUGGED_IN;
 267                                                 ul_logmsg("START_PLUGGED_IN\n");
 268                                         }
 269                                         break;
 270
 271                                 case START_PLUGGED_IN:
 272                                         robot.status[COMPONENT_START] = STATUS_OK;
 273                                         /* Competition starts when plugged out */
 274                                         if (instance->start_condition) {
 275                                                 FSM_SIGNAL(MAIN, EV_START, NULL);
 276                                                 robot.start_state = COMPETITION_STARTED;
 277                                                 ul_logmsg("STARTED\n");
 278                                         }
 279                                         break;
 280
 281                                 case COMPETITION_STARTED: {
 282                                         /* Subsequent plug-in stops the robot */
 283                                         static int num = 0;
 284                                         if (!instance->start_condition) {
 285                                                 robot.status[COMPONENT_START] = STATUS_WARNING;
 286                                                 if (++num == 10)
 287                                                         robot_exit();
 288                                         }
 289                                         break;
 290                                 }
 291                         }
 292                         last_instance = *instance;
 293                         break;
 294                 case DEADLINE:
 295                         robot.status[COMPONENT_START] = STATUS_FAILED;
 296                         DBG("ORTE deadline occurred - robot_cmd receive\n");
 297                         break;
 298         }
 299 }
 300
 301 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 302                         void *recvCallBackParam)
 303 {
 304         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
 305
 306         switch (info->status) {
 307                 case NEW_DATA: {
 308                         ROBOT_LOCK(hokuyo);
 309                         robot.hokuyo = *instance;
 310                         robot.status[COMPONENT_HOKUYO] = STATUS_OK;
 311                         ROBOT_UNLOCK(hokuyo);
 312                         if(!robot.ignore_hokuyo)
 313                         {
 314                                 update_map_hokuyo(instance);
 315                         }
 316                         break;
 317                 }
 318                 case DEADLINE:
 319                         robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
 320                         //system("killall -9 hokuyo");
 321                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 322                         break;
 323         }
 324 }
 325
 326 // FIXME: this is not up to date (Filip, 2010)
 327 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
 328                         void *recvCallBackParam)
 329 {
 330         struct camera_result_type *instance = (struct camera_result_type *)vinstance;
 331
 332         switch (info->status) {
 333                 case NEW_DATA: {
 334                         ROBOT_LOCK(camera_result);
 335                         robot.corns_conf_side = instance->side;
 336                         robot.corns_conf_center = instance->center;
 337                         ROBOT_UNLOCK(camera_result);
 338                         robot.status[COMPONENT_CAMERA] = STATUS_OK;
 339                         break;
 340                 }
 341                 case DEADLINE:
 342                         if (robot.orte.camera_control.on) {
 343                                 robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
 344                                 //system("killall -9 rozpuk");
 345                         }
 346
 347                         DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
 348                         break;
 349         }
 350 }
 351 /* ----------------------------------------------------------------------
 352  * SUBSCRIBER CALLBACKS - EB2008
 353  * ---------------------------------------------------------------------- */
 354
 355 void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
 356                             void *recvCallBackParam)
 357 {
 358         struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
 359         static int last_response_left = 0;
 360         static int last_response_right = 0;
 361         switch (info->status) {
 362                 case NEW_DATA:
 363                         // new data arrived and requested position equals actual position
 364                         if (instance->flags.left == 0 &&
 365                             instance->flags.right == 0)
 366                                 robot.status[COMPONENT_JAWS] = STATUS_OK;
 367                         else
 368                                 robot.status[COMPONENT_JAWS] = STATUS_WARNING;
 369
 370                         if (instance->response.left != last_response_left &&
 371                             instance->response.right != last_response_right &&
 372                             instance->response.left == act_jaw_left_get_last_reqest() &&
 373                             instance->response.left == act_jaw_right_get_last_reqest())
 374                                 FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
 375
 376                         last_response_left = instance->response.left;
 377                         last_response_right = instance->response.right;
 378                         break;
 379                 case DEADLINE:
 380                         robot.status[COMPONENT_JAWS] = STATUS_FAILED;
 381                         DBG("ORTE deadline occurred - actuator_status receive\n");
 382                         break;
 383         }
 384 }
 385
 386 void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
 387                             void *recvCallBackParam)
 388 {
 389         struct lift_status_type *instance = (struct lift_status_type *)vinstance;
 390         static int last_response = 0;
 391         switch (info->status) {
 392                 case NEW_DATA:
 393                         // new data arrived and requested position equals actual position
 394                         if (instance->flags == 0)
 395                                 robot.status[COMPONENT_LIFT] = STATUS_OK;
 396                         else
 397                                 robot.status[COMPONENT_LIFT] = STATUS_WARNING;
 398
 399                         if (instance->response != last_response &&
 400                             instance->response == act_lift_get_last_reqest())
 401                                 FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
 402                         last_response = instance->response;
 403                         break;
 404                 case DEADLINE:
 405                         robot.status[COMPONENT_LIFT] = STATUS_FAILED;
 406                         DBG("ORTE deadline occurred - actuator_status receive\n");
 407                         break;
 408         }
 409 }
 410
 411 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
 412                            void *recvCallBackParam)
 413 {
 414         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 415         static bool last_strategy;
 416         switch (info->status) {
 417                 case NEW_DATA:
 418                         robot.team_color = instance->team_color;
 419
 420                         if (!last_strategy && instance->strategy && (robot.start_state == POWER_ON)) {
 421                                         /* strategy switching */
 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_right_across || instance->bumper_left_across || instance->bumper_rear_left || instance->bumper_rear_right)
 439                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
 440
 441                         if (instance->bumper_left || instance->bumper_right) {
 442                                 FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
 443                         }
 444                         break;
 445                 case DEADLINE:
 446                         break;
 447         }
 448 }
 449
 450 #define HIST_CNT 5
 451 #if 0
 452 static int cmp_double(const void *v1, const void *v2)
 453 {
 454         const double *d1 = v1, *const d2 = v2;
 455         if (d1 < d2)
 456                 return -1;
 457         else if (d1 > d2)
 458                 return +1;
 459         else
 460                 return 0;
 461 }
 462 #endif
 463
 464 int robot_init_orte()
 465 {
 466         int rv = 0;
 467
 468         robot.orte.strength = 20;
 469
 470         rv = robottype_roboorte_init(&robot.orte);
 471         if (rv)
 472                 return rv;
 473
 474         /* creating publishers */
 475         robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
 476         robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
 477         robottype_publisher_est_pos_odo_create(&robot.orte, send_est_pos_odo_cb, &robot.orte);
 478         robottype_publisher_est_pos_indep_odo_create(&robot.orte, send_est_pos_indep_odo_cb, &robot.orte);
 479         robottype_publisher_est_pos_best_create(&robot.orte, send_est_pos_best_cb, &robot.orte);
 480         //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
 481
 482         // I didn't know what was the difference between the callback function pointer
 483         // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
 484         // IS CRUCIAL!
 485         //   - NULL: message is published only when OrtePublicationSend called
 486         //   - pointer to empty function: message is published periodically
 487         robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
 488         robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
 489         robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
 490         robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
 491         robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 492         robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 493
 494         /* create generic subscribers */
 495         robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
 496         robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
 497         robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
 498         robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
 499         robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
 500         //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
 501         robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
 502         robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
 503         robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
 504         robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
 505         robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
 506         robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
 507         robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 508
 509         return rv;
 510 }
 511