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