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